Robot motion control methods and devices

Abstract

The embodiment of the present application provides a robot motion control method and device, which relates to the field of robotics. The method includes: when the robot is currently in a first drag 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; when the robot is currently close to a singular posture, calculating a first virtual force based on 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 and whether the total torque is greater than a preset torque; when the total force in the first virtual force is not greater than a preset force or the total torque is not greater than a preset torque, calculating a target damping increase; and controlling the robot motion in the first drag mode based on the first virtual force and the target damping increase. In this way, the robot can be prevented from being in a singular posture and the degree of robot jitter caused by speed changes can be reduced.

Description

Technical Field

[0001] This application relates to the field of robotics, and more specifically, to a robot motion control method and apparatus. Background Art

[0002] Collaborative robots have been widely used in various 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, through advanced sensors, algorithms, and designs, achieve breakthroughs in safety and flexibility, collaborating directly with workers in the same workspace to complete production more efficiently.

[0003] For example, in the medical field, they assist in surgical procedures, help patients with rehabilitation training, and improve the quality of medical services. Furthermore, their applications in education, agriculture, and the service industry are becoming increasingly widespread, such as in experimental teaching, intelligent crop harvesting, and restaurant services. In all these scenarios, robot teaching is required to record locations.

[0004] Drag-and-drop teaching of collaborative robots is an intuitive and efficient method of robot programming. Through drag-and-drop teaching, users can directly guide the robot to a target location or execute a specific path by hand, without writing complex code. This method significantly reduces the demands on users when operating the robot, allowing even non-professionals to quickly master robot operation.

[0005] Currently, collaborative robot dragging is typically achieved through admittance control algorithms. However, in this method of human-robot interaction, the robot is prone to reaching singular poses. Near singular poses, the robot is in an unstable state, exhibiting joint overspeed, which affects the user experience and may even cause danger. Summary of the Invention

[0006] This application provides a robot motion control method, device, electronic device, and readable storage medium, which avoids the robot being in a strange posture by applying virtual force and adjusting damping, while reducing the degree of robot shaking that may be caused by speed changes.

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

[0008] In a first aspect, embodiments of this application provide a robot motion control method, the method comprising:

[0009] Under the current first drag mode based on admittance control, calculate the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot.

[0010] In the current near-singular pose, the first virtual force is calculated based on the first translation performance index and the first rotation performance index;

[0011] Determine 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;

[0012] If 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.

[0013] The robot's movement is controlled in the first dragging mode based on the first virtual force and the target damping increase.

[0014] Secondly, embodiments of this application provide a robot motion control device, the device comprising:

[0015] The index calculation module is used to calculate the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot when it is currently in the first drag mode based on admittance control.

[0016] The virtual force calculation module is used to calculate the first virtual force based on the first translation performance index and the first rotation performance index when the current near-singular pose is present.

[0017] The judgment module is used to determine 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;

[0018] The damping adjustment calculation module is used to calculate the target damping increase when 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.

[0019] The control module is used to control the robot's movement in the first dragging mode based on the first virtual force and the target damping increase.

[0020] Thirdly, embodiments of this application provide an electronic device, including 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 foregoing embodiments.

[0021] Fourthly, embodiments of this application provide a readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the robot motion control method as described in the foregoing embodiments.

[0022] The robot motion control method, device, electronic device, and readable storage medium provided in this application, when currently in a first dragging mode based on admittance control, calculates a first translational performance index and a first rotational performance index corresponding to the current joint angle of the robot. Then, when the robot is in a near-singular pose, a first virtual force is calculated based on the first translational performance index and the first rotational performance index. Next, it determines whether the total force magnitude and total torque magnitude of the first virtual force are greater than a preset force and a preset torque, respectively. If it is determined that the total force magnitude or total torque magnitude of the first virtual force is not greater than the preset force or the preset torque magnitude is not greater than the preset torque, a target damping increase is calculated. Based on the first virtual force and the target damping increase, the robot's motion is controlled in the first dragging mode. Thus, by applying virtual forces and adjusting damping, the robot is prevented from being in a singular pose, while simultaneously reducing the degree of robot jitter that may be caused by speed changes. Attached Figure Description

[0023] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

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

[0025] Figure 2 This is one of the flowcharts illustrating the robot motion control method provided in the embodiments of this application;

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

[0027] Figure 4 This is a second schematic flowchart of the robot motion control method provided in the embodiments of this application;

[0028] Figure 5 for Figure 2 A flowchart illustrating the sub-steps included in step S130;

[0029] Figure 6 The third schematic flowchart of the robot motion control method provided in the embodiments of this application;

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

[0031] Figure 8The fourth flowchart illustrates the robot motion control method provided in this application embodiment;

[0032] Figure 9 This is a block diagram of a robot motion control device provided in an embodiment of this 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 Implementation

[0034] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0035] Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0036] It should be noted that relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0037] Currently, there are two methods for teaching collaborative robots to drag: current loop-based dragging and force sensor-based dragging. Current loop-based dragging requires establishing a dynamic model of the collaborative robot, and the dragging effect is affected by joint friction, resulting in a poor dragging experience. Force sensor-based dragging, on the other hand, requires force sensors and admittance control algorithms, directly acting on the position loop of the motor. When the user applies external force, the robot can sense it and adjust its movement in real time, thus achieving a smooth and natural dragging process and enabling 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 (including external force and torque) sensed by the sensor at the current moment; x e (t) represents the pose increment generated by the admittance control algorithm at the current moment; x e (t-1) represents the pose increment generated by the admittance control algorithm at the previous moment; This represents the velocity generated by the admittance control algorithm at the previous moment. This represents the velocity at the current moment generated by the admittance control algorithm; ΔT represents the acceleration generated by the external force at the current moment; ΔT represents the integration time.

[0041] However, admittance control algorithms have the following drawbacks. During human-computer interaction, robots are prone to reaching singular poses. Near singular poses, the robot is unstable, exhibiting joint overspeed, affecting user experience, and potentially causing danger. Currently, most researchers use two methods to avoid this danger. Method 1: Stop the robot's movement near the singular pose. This method is simple and crude, requiring manual movement away from the singular point (i.e., the singular pose) each time it reaches one, which is cumbersome. Method 2: Apply a virtual force to the robot's end effector as it approaches the singular pose to avoid danger. While Method 2 doesn't require manual movement away from the singular pose each time, suddenly applying a virtual force when approaching (i.e., about to reach) the singular pose may cause discontinuous acceleration, leading to severe robot shaking, misjudging the robot's operation by the user, and hindering the promotion of collaborative robots. Therefore, how to avoid the singularity problem of admittance control algorithms and find a stable, safe, and smooth collaborative robot teaching method is a problem that researchers urgently need to solve.

[0042] In view of the above situation, this application provides a robot motion control method, device, electronic device and readable storage medium, which avoids the robot from being in a strange posture by applying virtual force and adjusting damping, and at the same time reduces the degree of robot shaking that may be caused by speed changes.

[0043] The following detailed description of some embodiments of this application is provided in conjunction with the accompanying drawings. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0044] Please refer to Figure 1 , Figure 1 This is a block diagram of an electronic device 100 provided in an embodiment of this application. The electronic device 100 may be, but is not limited to, a robot, a robot control device, etc. The electronic device 100 may include a memory 110, a processor 120, and a communication unit 130. The memory 110, processor 120, and communication unit 130 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components can be electrically connected to each other through 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, random access memory (RAM), read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), 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, which 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 the software programs and modules stored in the memory 110, such as the robot motion control device 200 in this embodiment, thereby realizing the robot motion control method in this embodiment.

[0047] The communication unit 130 is used to establish a communication connection between the electronic device 100 and other communication terminals through the 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 electronic device 100. The electronic device 100 may also include components that are larger than... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown. Figure 1 The components shown can be implemented using hardware, software, or a combination thereof.

[0049] Please refer to Figure 2 , Figure 2 This is one of the flowcharts illustrating a robot motion control method provided in this application. The method can be applied to the aforementioned electronic device and executed by it. The specific flow of the robot motion control method is described in detail below. In this embodiment, the method may include steps S110, S130 to S150, and S170.

[0050] Step S110: Under the current first drag mode based on admittance control, calculate the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot.

[0051] In this embodiment, when the robot is currently operating based on the admittance control algorithm, i.e., in the first drag mode based on admittance control, the first translational performance index (also known as the first rotational performance index) and the first rotational performance index can be calculated based on the joint angles of each joint 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. The first translational performance index and the first rotational performance index can be calculated using the following formula (2). Formula (2) divides the Jacobian matrix into the translational direction (i.e., the translational direction) and the rotational direction, thereby decoupling the translational and rotational directions.

[0052]

[0053] in, Represents the Jacobian matrix; This represents the pseudo-inverse of the Jacobian matrix in the translational direction; The pseudo-inverse of the Jacobian matrix representing the direction of rotation; This represents the transpose of the Jacobian matrix in the translational direction; I represents the transpose of the Jacobian matrix indicating the direction of rotation; w represents the identity matrix. T (q) represents the first translational performance index; w R(q) represents the first rotational performance index.

[0054] Step S130: Under the current near-singular pose condition, calculate the first virtual force based on the first translation performance index and the first rotation performance index.

[0055] Whether a robot is approaching a singular pose can be determined in any way. For example, it can be determined based on human input, or by analyzing the robot's current state. Once it is determined that the robot is approaching a singular pose and its first translational and first rotational performance indices are obtained, a first virtual force can be calculated based on these indices.

[0056] Step S140: Determine 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.

[0057] Step S150: If 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.

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

[0059] In this embodiment, upon obtaining the first virtual force, it can be determined whether the total force magnitude of the first virtual force is greater than a preset force, and whether the total torque magnitude of the first virtual force is greater than a preset torque. The total force magnitude of the first virtual force is determined based on the forces in each direction of the first virtual force, and the total torque magnitude of the first virtual force is determined based on the torques in each direction of the first virtual force. The specific magnitudes of the preset force and preset torque can be determined according to actual needs and are not specifically limited here.

[0060] The first virtual force only plays a dominant role in moving away from singular poses when the total force of the first virtual force is greater than a preset force or the total torque of the first virtual force is greater than a preset torque. That is, if 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; if 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 both the total force and the total torque of the first virtual force are greater than the preset force, the robot's movement can be controlled solely based on the first virtual force in the first dragging mode, without needing to adjust 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 based on the target damping increase and the original damping, the damping used in admittance control (i.e., adaptive damping) is calculated, and then the robot motion is controlled in the first drag mode based on the first virtual force and the calculated damping used in admittance control.

[0062] As shown in formula (1) above, admittance control is obtained by integrating the velocity from the previous moment to obtain the velocity at the current moment. When it is determined that the current position is close to a singular pose, a virtual force is generated, and the direction of the virtual force is opposite to the direction of the velocity at the current moment; otherwise, the robot would move to the singular pose. If the virtual force is applied directly at this time, the admittance control will experience a sudden acceleration change, which in turn causes a velocity change, resulting in severe shaking of the robot. Moreover, the faster the velocity at the current moment, the more obvious the shaking phenomenon. The reason for the above situation is that the velocity at the current moment needs to be reversed, but the velocity at the previous moment needs to be decayed to 0 and then accelerated in the opposite direction, which will result in severe shaking.

[0063] In this embodiment, a virtual force threshold (i.e., the aforementioned preset force and preset torque) and adaptive damping are introduced to rapidly decay the velocity from the previous moment, thereby reducing jitter. Even if the current velocity generated by admittance control is too large, it can still be suppressed.

[0064] In this embodiment, the six-dimensional force sensor is first installed on the robot's end flange, and the zero-point calibration of the force sensor is then completed. Furthermore, when the direction of the six-dimensional force sensor is inconsistent with the direction of the end flange, for example... Figure 3 In the indicated state, the transformation relationship from the force sensor coordinate system to the end-effector coordinate system (i.e., the end flange coordinate system) [xy za bc] 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, a homogeneous transformation matrix from the force sensor coordinate system to the end-effector coordinate system can be established. As shown in formula (3).

[0065]

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

[0067] During normal dragging based on force sensors, the sensor data detected by the six-dimensional force sensor (used to indicate external force information) and the aforementioned homogeneous transformation matrix can be used as a basis. The robot obtains sensor data in the end-effector coordinate system and then controls its movement based on this data. For example, admittance control can be used to move the robot based on sensor data in the end-effector coordinate system.

[0068] For example, suppose the data in the force sensor coordinate system 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 end coordinate system is F E F E As shown in formula (4).

[0069]

[0070] Where, 0∈R 3×3 It is a zero matrix.

[0071] In the admittance-based control algorithm operation, the first translational performance index w of the robot at the current position can be calculated based on the above formula (2). T (q) and the first rotational performance index w R (q).

[0072] As one possible implementation, such as Figure 4 As shown, the translation performance index w can be used as a reference. T (q) and the first rotational performance index w R (q) Determine whether the robot is currently approaching a singular pose. Please refer to... Figure 4 , Figure 4 This is a second schematic flowchart of a robot motion control method provided in an embodiment of this application. In this embodiment, after step S110, the method may further include steps S121 to S124.

[0073] Step S121: Determine whether the first translation performance index is within the preset translation performance index range.

[0074] Step S122: Determine whether the first rotational performance index is within the preset rotational performance index range.

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

[0076] Step S123: Determine that the current position is not close to a singular pose.

[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, then step S124 is executed.

[0078] Step S124: Determine the current near-singular pose.

[0079] In this embodiment, the possible singular poses of the robot can be analyzed and determined based on the number of joints in the robot. Then, the minimum value w of the preset translational performance index range can be determined based on the translational and rotational performance indices corresponding to the different possible singular poses of the robot. Tmin and the minimum value w of the preset rotational performance index range Rmin Furthermore, the maximum value w of the preset translational performance index range is determined by controlling the robot to move away from different possible singular poses and their corresponding translational and rotational performance indices. Tmax and the maximum value w of the preset rotational performance index range Rmax The minimum value among the translation performance indices corresponding to different singular poses can be used as the minimum value of the preset translation performance index range, and the minimum value among the rotation performance indices corresponding to different singular poses can be used as the minimum value of the preset rotation performance index range; the maximum value among the translation performance indices corresponding to movement away from the singular pose from the possible different singular poses can be used as the maximum value of the preset translation performance index range; similarly, the maximum value among the rotation performance indices corresponding to movement away from the singular pose from the possible different singular poses can be used as the maximum value of the preset rotation performance index range.

[0080] As one 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, translational and rotational performance indices corresponding to each of these singularities are measured through prior experiments. The minimum value among these translational performance indices is taken as the minimum value of a preset translational performance index range, and the minimum value among these rotational performance indices is taken as the minimum value of a preset rotational performance index range. Furthermore, the robot is moved from the shoulder singularity direction away from the shoulder singularity, and the translational and rotational performance indices after the movement are obtained; the robot is moved from the elbow singularity direction away from the elbow singularity, and the translational and rotational performance indices after the movement are obtained; the robot is moved from the wrist singularity direction away from the wrist singularity, and the translational and rotational performance indices after the movement are obtained. Then, the maximum value among the translational performance indices after the above three movements is taken as the maximum value of the preset translational performance index range, and the maximum value among the above rotational performance indices is taken as the maximum value of the preset rotational performance index range.

[0081] The minimum value w of the preset translation performance index range Tmin As the second translation performance index, the maximum value w within the preset translation performance index range. 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 within 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 At that time, i.e., w T (q)>w Tmax At this time, no virtual force in the translational direction may be generated; in the first rotational performance index w R (q) is greater than the third rotational performance index w Tmax At that time, i.e., w R (q)>w Rmax At this time, no virtual force in the rotational direction may be generated. Therefore, if the first translational performance index is not within the range of the preset translational performance index and the first rotational performance index is not within the range of the preset rotational performance index, it can be determined that the robot is currently in a draggable area and can be operated at will. It is not currently approaching a singular pose 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) When the preset translation performance index is within the range, i.e., w Tmin <w T (q)≤w Tmax At this time, a virtual force in the translational direction needs to be generated to resist the robot from reaching a singular pose. In the first rotational performance index w R (q) When the preset rotational performance index is within the range, i.e., w Rmin <w R (q)≤w Rmax At this time, a virtual force in the direction of rotation needs to be generated to resist the robot from reaching a singular pose. That is, if the first translation performance index is within the range of the preset translation performance index or the first rotation performance index is within the range of the preset rotation performance index, then it is determined that the robot is approaching a singular pose, and the first virtual force is calculated.

[0084] As one possible implementation method, it can be achieved through Figure 5 The first virtual force is calculated as shown. Please refer to... Figure 5 , Figure 5 for Figure 2A flowchart illustrating the 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 Cartesian space, calculate the first joint angle and the second joint angle for the next moment based on the unit forward and reverse velocity of that direction, the preset duration, and the current joint angle corresponding to that direction.

[0086] Sub-step S132: Obtain the first performance index corresponding to the first joint angle and the second performance index corresponding to the second joint angle.

[0087] Sub-step S133: Based on the first performance index and the second performance index corresponding to the direction, obtain the gradient direction value corresponding to the direction.

[0088] Sub-step S134: Calculate the first virtual force based on the gradient direction values ​​corresponding to each direction in 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 translational performance index and the first rotational performance index, and the gradient direction of the first virtual force can be calculated. 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 can be calculated based on formula (5). The magnitude of the virtual force calculated by formula (5) is non-linear; the closer to the singular pose, the greater the virtual force, 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: the first translation performance index w T (q) corresponds to k T (w) and the first rotational performance index w R (q) corresponds 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 The components corresponding to the translation direction in (w) and A(q) are calculated, and based on k R The components corresponding to the rotation direction in A(w) and A(q) are calculated to obtain the first virtual force f1 with the same number of dimensions as A(q).

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

[0094]

[0095] Where λ 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 Admittance control algorithms operate in Cartesian space, while performance metrics calculations require joint angles. Therefore, unit forward and reverse velocities can be generated based on the directions in Cartesian space to calculate the robot's joint angles at the next moment, thereby determining which direction the admittance control algorithm should control the robot to move in.

[0097] In this embodiment, we first take the X-direction in Cartesian space as an example to introduce how to obtain the gradient direction value A in the X-direction of Cartesian space. 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 velocity in the X direction of Cartesian space, the preset duration, and the current joint angle (i.e. the current joint angle of the end joint) corresponding to the X direction of Cartesian space.

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

[0100] Where J represents the Jacobian matrix, used to express the terminal linear velocity (in formula (7), this 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 q represents the unit forward and reverse velocity in the X direction of Cartesian space; ± This represents the robot joint angles caused by unit velocity, specifically the first and second joint angles at the next instant corresponding to the X-direction in Cartesian space.

[0101] Then, the first performance index can be calculated based on the first joint angle corresponding to the X direction in Cartesian space at the next moment, and the second performance index can be calculated based on the second joint angle corresponding to the X direction in Cartesian space at the next moment. That is, based on q ± The performance index w was calculated. T (q± Next, based on performance metrics w T (q ± The gradient direction value A corresponding to the X direction in Cartesian space is calculated. x .

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

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

[0104] If 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 in the X direction of the Cartesian space is determined to be 0. Here, the corresponding value is the index that corresponds to the first translation performance index among the first translation performance index and the first rotation performance index. For example, if the first performance index corresponds to a translation direction, i.e., the X, Y, Z directions, then the corresponding value of the first performance index is the first translation performance index; if the first performance index corresponds to a rotation direction, i.e., the RX, RY, RZ directions, then the corresponding value of the first performance index is the first rotation performance index.

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

[0106] In other words, 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 unit positive velocity along the X-axis in Cartesian space, w T (q - (w) represents the second performance index corresponding to the unit reverse velocity along the X-axis in Cartesian space; T (q + )-w T (q) represents the first difference; (w) T (q - )-w T (q)) represents the second difference.

[0109] Then, the unit forward and reverse velocities in the Cartesian space Y, Z, RX, RY, and RZ directions are calculated sequentially 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 The following methods can be used to determine whether the total force in the first virtual force is greater than the preset force and whether the total torque in the first virtual force is greater than the preset torque. Then, based on the relationship between the total force in the first virtual force and the preset force, and or the relationship between the total torque in the first virtual force and the preset torque, it can be determined whether the target damping increase needs to be calculated, and when the target damping increase needs to be calculated, which dimensions of the target damping increase need to be calculated. The dimensions that need to be calculated are the target dimensions.

[0111] Can be judged as well as Whether it holds true. When When the total force in the first virtual force is greater than the preset force, the force in the first virtual force f1 plays a dominant role in moving away from the singular pose, without needing to adjust the damping corresponding to the translation direction. When the total torque in the first virtual force is greater than the preset torque, the torque in the first virtual force f1 plays a dominant role in moving away from the singular pose, without needing to adjust the damping corresponding to the rotation direction.

[0112] exist When this happens, it is necessary to adjust the damping corresponding to the translation direction, that is, to calculate the target damping increase, and the X, Y, and Z directions of the target damping increase are the target dimensions of the value to be calculated. In other words, the X, Y, and Z components of the target damping increase need to be calculated. When this happens, it is necessary to adjust the damping corresponding to the rotation direction, that is, 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. In other words, the components in the RX, RY, and RZ directions in the target damping increase need to be calculated.

[0113] Therefore, it can be concluded that in or When calculating the target damping increase, it is necessary to determine the target damping increase and then introduce adaptive damping. This adaptive damping is calculated based on the original damping and the calculated target damping increase. Specifically, when calculating the target damping increase, if there is no need to adjust the damping corresponding to the translation direction, the values ​​of each dimension (i.e., X, Y, Z directions) of the target damping increase corresponding to the translation direction are 0. That is, the component of the target damping increase corresponding to the translation direction is 0, and only the values ​​of each dimension (i.e., RX, RY, RZ directions) of 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 each dimension (i.e., RX, RY, RZ directions) of the target damping increase corresponding to the rotation direction are 0. That is, the component of the target damping increase corresponding to the rotation direction is 0, and only the values ​​of each dimension (i.e., X, Y, Z directions) of the target damping increase need to be calculated.

[0114] When calculating the target damping increase, for each target dimension in the target damping increase, if the product of the force or torque corresponding to that target dimension in the first virtual force and the current velocity corresponding to that target dimension is greater than 0, then the component of that 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; 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 velocity corresponding to a target dimension can be the velocity of the robot's end effector in that target dimension.

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

[0116] The following example, using the X direction as a target dimension, illustrates how to obtain the X-direction component of the target damping increase.

[0117] Assuming 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] Where k is a constant used to control the overall decay rate; β>1, the faster the current speed, the greater the damping, causing it to decay rapidly.

[0120] When using the above formula (9), when the force in the X direction of the first virtual force is equal to the current velocity v in this direction... x If the direction of (t) is opposite, the value of the target damping increase in the X direction is greater than 0, thereby increasing the damping used in admittance control to reduce the current speed of 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. Based on the adaptive damping and the first virtual force, the robot's motion is controlled to avoid singular poses when using the admittance control algorithm. Optionally, the first virtual force can be directly used as the external force detected in the admittance control algorithm (i.e., as the external force F in the above formula (1)) for control. In this embodiment, the default singular pose handling strategy is avoidance. When the singular pose handling strategy is avoidance, steps S110, S130 to S150 and S170 can be executed.

[0122] Admittance control algorithms can sometimes lead to singular poses and instability in the robot, which can be avoided by applying virtual forces. However, since the virtual force acts in the opposite direction to the force from the actual force sensor, Newton's second law states that under the influence of the admittance control algorithm, this can cause a sudden change in the robot's acceleration, resulting in severe shaking. To address this, this embodiment incorporates adaptive damping and a virtual force buffer. This not only prevents robot shaking caused by sudden changes in virtual force but also prevents the avoidance algorithm from failing due to excessive actual force.

[0123] To increase flexibility, this embodiment allows for two singular pose handling strategies: singular pose traversal (also known as singular point traversal) and singular pose avoidance (also known as singular point avoidance). A singular pose handling strategy can be automatically selected, and processing can be performed according to that strategy when approaching a singular pose; alternatively, the user can select a singular pose handling strategy, and processing can be performed according to that strategy when approaching a singular pose.

[0124] Please refer to Figure 6 , Figure 6 This is the third flowchart illustrating the robot motion control method provided in this application embodiment. In this embodiment, the method may further include steps S101 and S182. Specifically, when the robot is currently in a first dragging mode based on admittance control, step S101 is executed before calculating the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot.

[0125] Step S101: Determine whether the current singular pose handling strategy is avoidance or traversal.

[0126] When the robot is currently in the first drag mode based on admittance control and the current singular pose handling strategy is avoidance, the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot can be calculated, and then steps S130 to S150 and step S170 can be executed.

[0127] If the current state is in the first drag mode based on admittance control and the current singular pose handling strategy is traversal, step S182 can be executed.

[0128] Step S182: If the current singular pose processing strategy is to traverse, and it is determined that the current approach is a singular pose, then 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] In the current admittance-based first drag mode and with the current singular pose handling strategy being traversal, it can be determined in any way whether the robot is approaching a singular pose. For example, it can be determined based on human input, or by analyzing the robot's current state to determine if it is approaching a singular pose. For instance, this can be done using the aforementioned first translational performance index and first rotational performance index. If it is determined that the robot is approaching a singular pose, the current mode can be switched from the admittance-based first drag mode to the current loop-based second drag mode, and then the robot's movement can be controlled in this second drag mode to traverse the singular pose.

[0130] Please refer to this again. Figure 6 After step 182, the method may further include step S183.

[0131] Step S183: After switching to the second drag mode, if it is determined that the robot is far from the singular pose, 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 that the robot is far from the singular pose after control, 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's movement can be controlled in the first drag mode.

[0133] Please refer to this again. Figure 6As a possible implementation, when the current singular pose handling strategy is traversal, the method may further include step S181, which can be achieved through... Figure 6 In step S181, it is determined whether the robot is currently close to a singular pose.

[0134] Step S181: Based on the current joint state of the robot and the joint angle range or preset distance corresponding to at least one singular pose, determine whether the robot is currently close to a singular pose.

[0135] like Figure 7 As shown, there are three common singularity types in collaborative robots: elbow singularity, wrist singularity, and shoulder singularity. Elbow and wrist singularities in collaborative robots correspond to joints 3 and 5 having angles close to 0, respectively. Shoulder singularity occurs because the center of the wrist joint is collinear with the axis of the first joint. Whether it is close to a shoulder singularity can be determined by the distance *m*, where *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] Where d6 represents the translational displacement of the 6th joint along the Z-direction of the 5th joint; d4 represents the translational displacement of the 4th joint along the Z-direction of the 3rd joint.

[0138] The homogeneous transformation matrix of the robot end-effector coordinate system relative to the base coordinate system is shown in Equation (11).

[0139]

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

[0141] In this embodiment, based on the above analysis, the range of joint angles or preset distances corresponding to the various singular poses that the robot may exhibit can be determined. Then, based on the current joint state of the robot and the range of joint angles or preset distances corresponding to at least one singular pose, it can be accurately determined whether the robot is currently close to a singular pose.

[0142] For example, if a singular pose can be determined based on a range of joint angles, then the relationship between the robot's current angle and the range of joint angles can be used to determine whether the robot is approaching a singular pose. Similarly, if a singular pose can be determined based on distance, then the relationship between the robot's actual distance and a pre-set distance can be used to determine whether the robot is approaching a singular pose.

[0143] For example, the robot is a six-DOF collaborative robot. The joint angle range corresponding to the elbow singularity can be predetermined as the joint angle range of joint 3. If the current joint angle q3 of joint 3 is within the predetermined joint angle range of joint 3, i.e., |q3|≤q... 3lim Then it is determined that the robot is currently approaching a singular pose, where -q 3lim ~+q 3lim This represents 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 robot's current joint 5 is within the predetermined joint angle range of joint 5, i.e., |q5|≤q 5lim Then it is determined that the robot is currently approaching a singular pose, where -q 5lim ~+q 5lim This represents the range of joint angles for joint 5 corresponding to the wrist singularity. Similarly, the preset distance m corresponding to the shoulder singularity 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 This indicates that the robot is currently approaching a singular pose.

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

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

[0146]

[0147] Where τ is the link torque obtained from dynamic calculation, representing the joint torque that needs to be applied to maintain the current posture of the robot in the current loop-based drag mode; q is the rotation angle at the link end; M(q) is the inertia matrix; Let G(q) represent the Coriolis force and centrifugal force, and let G(q) represent the gravity term.

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

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

[0150] Where, τ m τ is the driving torque of the motor. f This refers to the joint friction torque;

[0151] In the second drag mode based on the current loop, the term that is difficult to model is the external force that the user needs to apply. As a possible implementation, 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 the current loop drag.

[0152]

[0153] Where, λ τ To drag the gain.

[0154] External force is detected by force sensor, and the force is converted to the joint through force Jacobian matrix. Then, it is combined with dynamic model, and formula (14) can be reorganized into formula (15).

[0155]

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

[0157] At this point, current loop-based drag can be achieved.

[0158] The dragging effect can be determined by the parameter λ. τ To determine this, the specific parameter λ can be set according to actual needs. τ The value of λ. τ,i When λ = 0, the dragging force is resisted, and the corresponding joint i of the robot cannot be dragged, achieving a joint locking effect. When 0 < λ τ,i When λ < 1, the robot has difficulty dragging, and λ τ,i The smaller the value, the harder it is to drag. In λ τ,i When λ = 1, there is no compensation effect, equivalent to the drag effect in normal torque mode. τ,i When the value is greater than 1, it compensates for dragging force, improving the dragging experience, and λ τ,i The larger the size, the easier it is to drag.

[0159] In this embodiment, after switching to the second dragging mode for control, if it is determined that the robot is currently far from the singular pose, the control can be switched back to the first dragging mode. Correspondingly, whether the robot is currently far from the singular pose can be determined based on the approach to the singular pose used when switching to the second dragging mode. For example, if the angles of joints 3 and 5 are not within the corresponding preset angle range, i.e., |q3|≤q... 3lim |q5|≤q 5lim None of these conditions are met, and the distance m is not less than the preset distance m. min That is, m≤m min If the condition is not met, it is determined that the current position is far from the singular pose, 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] Because collaborative robot joint encoders have high precision, to prevent frequent switching between modes from affecting the dragging experience, the mode switching decision considers not only whether the current position is moving away from or towards a singular pose, but also the linear and angular velocities of the robot's end effector (i.e., the robot's TCP (Tool Center Point) linear and angular velocities). In other words, the decision to switch between the first and second dragging modes is based on whether the current position is moving away from or towards a singular pose, as well as the linear and angular velocities of the robot's end effector.

[0161] Optionally, if the robot is currently in the first dragging mode and is approaching a singular pose, and the linear velocity and angular velocity of the robot's end effector are both less than a preset linear velocity and a preset angular velocity, the robot can switch from the first dragging mode to the second dragging mode, i.e., the admittance control mode can be switched to the current loop dragging mode; otherwise, no switching occurs. If the linear velocity and angular velocity of the robot's end effector are both less than the preset linear velocity and a preset angular velocity, the robot is considered to be about to come to a standstill; if the linear velocity or angular velocity of the robot's end effector is not less than the preset linear velocity, or is not less than the preset angular velocity, the robot is considered to be moving.

[0162] Furthermore, if the robot is currently in the second drag mode and is far from the singular pose, 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, the robot can switch from the second drag mode to the first drag mode, i.e., the current loop drag mode is switched to the admittance control mode; otherwise, no switching is performed.

[0163] The admittance control algorithm enables smooth human-computer interaction, i.e., drag-and-drop teaching. Taking the elbow singularity of a collaborative robot as an example, a singularity avoidance strategy is used, but an angular difference always exists between the 2-axis and 3-axis, constraining the collaborative robot's workspace. In this embodiment, a singular pose traversal strategy is adopted, allowing the robot to be dragged and taught to any location in the workspace.

[0164] For the handling strategy of the admittance control algorithm near singular positions, this embodiment provides two strategies: avoidance and crossing. In the avoidance strategy, to avoid the severe jitter and directional coupling problems of current common processing methods, the Jacobian matrix is ​​divided into two directions, translation (i.e., translational movement) and rotation (i.e., rotational movement), and processed separately. Moreover, since the inverse of the Jacobian matrix does not exist or has low accuracy near singular poses, the pseudo-inverse of the Jacobian matrix is ​​used to calculate the virtual force direction. At the same time, adaptive damping and a virtual force threshold are introduced. When a virtual force is generated but does not exceed the threshold, the damping coefficient of the control algorithm is adjusted according to the current velocity of the admittance control, so that the velocity decays rapidly; when the virtual force exceeds the threshold, the virtual force takes the dominant role and moves away from the singular point.

[0165] When a traversal strategy is selected, the robot switches modes based on its joint angles. When the joint angle is less than the designed joint angle threshold, admittance control switches to a current loop-based drag mode. Simultaneously, a current loop-based drag control law is designed, introducing drag gain to adjust the dragging effect.

[0166] Collaborative robots have joint hard limits, and under admittance control algorithms, there is a risk that the joints may reach these hard limits. In this embodiment, to prevent the robot from reaching the joint hard limit, a second virtual force is applied when approaching the hard limit to avoid damaging the robot.

[0167] Please refer to Figure 8 , Figure 8 This is a fourth schematic flowchart illustrating the robot motion control method provided in this embodiment. 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, calculate the second virtual force used to constrain the robot to reach the joint hard limit region.

[0169] In this embodiment, the hard limit buffer is used to represent the angle region that is about to reach the joint hard limit. It can be determined in any way whether the robot currently has a target joint angle within the hard limit buffer. If the robot currently has no target joint angle within the hard limit buffer, the second virtual force does not need to be calculated. If the robot currently has a target joint angle within the hard limit buffer, the second virtual force used to constrain the robot to reach the joint hard limit can be calculated. The target joint is a pre-specified joint that needs to be determined whether it is within the hard limit buffer. Specifically, it can be determined based on actual needs; for example, it can be all joints of the robot or only some joints of the robot.

[0170] Given the calculated second virtual force and first virtual force, the robot's movement can be controlled in the first dragging mode based on the first virtual force, the second virtual force, and the target damping increase.

[0171] Please refer to this 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, determine whether the current joint angle of the target joint is within the hard limit buffer zone based on 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 The number 'i' corresponds to the number of joints in 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 Movement between joints. Based on the hard limit angle q of each joint. imin q imax The hard limit buffer zone of each target joint can be determined [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 within the hard limit buffer, the virtual force in the joint space corresponding to the target joint can be calculated based on the current joint angle of the target joint and the hard limit buffer it is in. Then, based on the virtual force in the joint space corresponding to each target joint of the robot, the second virtual force can be calculated.

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

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

[0177]

[0178] Where α is the virtual force gain.

[0179] The calculated virtual forces in the joint space can be transformed to the base coordinate system using the force Jacobian matrix. For example, by using formula (17), the virtual forces in the joint space corresponding to joint 6, f6, can be transformed to the base coordinate system to obtain 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 This represents the sixth column of the matrix.

[0182] The virtual force in the base coordinate system is transformed to the end flange coordinate system. For example, f is transformed using formula (18). 6base Transform to the end coordinate system to obtain 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 within the hard limit buffer zone, the virtual force in the joint space corresponding to that 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 virtual forces in the joint space corresponding to each target joint of the robot. When the robot currently has a target joint within the hard limit buffer zone, is approaching a singular pose, and requires damping adjustment, the robot's movement can be controlled in the first dragging 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 joint hard limit and the virtual force generated by the singular pose region, they work together on the admittance control algorithm, causing the robot to move away from the singular pose and hard limit point, thus avoiding instability of the robot system caused by the above phenomena.

[0186] In this embodiment, when the current position is not close to an unusual pose but the joint angles are within a hard limit buffer zone, the robot's movement can be controlled in the first dragging mode based on the second virtual force. If it is determined that the total force magnitude in the first virtual force is greater than a preset force and the total torque magnitude is greater than a preset torque, the robot's movement can be controlled in the first dragging mode based on the first virtual force and the second virtual force.

[0187] In the above approach, to avoid conflicts between the virtual forces generated by Cartesian space constraints and the virtual forces in joint space, only the rotation direction of the collaborative robot is constrained. In this case, the target joints do not include joints that can be in singular poses and correspond to the rotation direction. For example, when the robot is a six-DOF collaborative robot, joints 4, 5, and 6 correspond to the rotation direction (i.e., the rotation direction), and joint 5 may exhibit a wrist singularity. Therefore, joints 4 and 6 can be used as target joints, and the virtual forces in joint space of target joints 4 and 5 can be calculated. Then, the virtual forces generated by the joint hard constraint are summed to obtain the virtual forces.

[0188] Understandably, if it is determined that the joint angles of the robot without joints are located in the hard limit buffer and are not currently approaching a singular pose, normal dragging based on force sensors can be performed under the admittance control algorithm.

[0189] Admittance control algorithms operate in Cartesian space, and singular poses also occur within Cartesian space. For cooperative machines, some joints have hard limits; when admittance control is applied, joint angles may reach these hard limits. Therefore, in this embodiment, a virtual force considering the joint hard limits is added. When a joint angle reaches the buffer zone of the hard limit, a virtual force is generated based on the joint angle. This force is then transformed to the base coordinate system via a force-Jacobi matrix, and finally to the end-effector coordinate system. This force, added to the virtual force generated in the singular pose region, acts together in the admittance control algorithm, achieving stability and safety in human-machine interaction.

[0190] In the above embodiments, an admittance control algorithm for singular pose avoidance based on adaptive damping and virtual force is provided. To avoid robot jitter during singular pose avoidance, adaptive damping and a virtual force threshold are introduced. When a virtual force is generated but does not exceed the virtual force threshold, the current velocity is rapidly decayed to zero according to the admittance control velocity magnitude. Subsequently, under the action of the virtual force, the robot accelerates away from the singular point. An admittance control method capable of traversing singular poses is also provided: a judgment is made based on the robot's actual angle and the designed joint angle threshold. When the joint angle is less than the set threshold, the admittance control switches to a current loop-based dragging mode; simultaneously, force sensor data is transformed to the joint and added to the dynamic model to design a new dragging control law, and the dragging effect is adjusted by the dragging gain. Meanwhile, a control method for constraining robot hard limits is also provided: a hard limit buffer is established based on the joint hard limit angle. Under the action of the admittance control algorithm, if the joint angle is within the hard limit buffer, 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 pose region and acts together on the admittance control algorithm. This not only prevents the robot from reaching the hard limit, but also prevents the robot from reaching the singular pose, thus ensuring the stability and safety of the robot system.

[0191] The robot control method provided in the above embodiments is an admittance control algorithm based on a six-dimensional force sensor. When users perform dragging operations or point-to-point teaching using the six-dimensional force sensor, it avoids robot instability caused by algorithm defects, thus improving the safety of human-computer interaction. Without affecting the admittance control algorithm, it can improve the efficiency of force sensor-based operations and enhance the user experience.

[0192] To perform the corresponding steps in the above embodiments and various possible methods, an implementation of a robot motion control device 200 is given below. Optionally, the robot motion control device can adopt the above-described... Figure 1 The device structure of the electronic device 100 shown. Further, please refer to... Figure 9 , Figure 9 This is a block diagram of the robot motion control device 200 provided in this embodiment. 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 in the above embodiments. For the sake of brevity, any parts not mentioned in this embodiment can be referred to the corresponding content in the above 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 translational performance index and the first rotational performance index corresponding to the current joint angle of the robot when it is currently in the first drag mode based on admittance control.

[0194] The virtual force calculation module 220 is used to calculate the first virtual force based on the first translation performance index and the first rotation performance index when the current near-singular pose is present.

[0195] The judgment module 230 is used to determine 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.

[0196] The damping adjustment calculation module 240 is used to calculate the target damping increase when 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.

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

[0198] In this embodiment, the judgment module 230 can also be used to determine whether the current position is close to a singular pose based on the first translation performance index and the first rotation performance index.

[0199] In this embodiment, the control module 250 is further configured to determine whether the current singular pose handling strategy is avoidance or passage when the robot is currently in a first dragging mode based on admittance control. If the current singular pose handling strategy is avoidance, the index calculation module 210 calculates the first translational performance index and the first rotational performance index corresponding to the robot's current joint angle. If the current singular pose handling strategy is passage, and it is determined that the robot is approaching a singular pose, the control module 250 switches the current mode from the first dragging mode to a second dragging mode based on a current loop, and controls the robot's movement in the second dragging mode.

[0200] When the current singular pose handling strategy is to pass through, the judgment module 230 is further configured to: determine whether the robot is currently approaching a singular pose based on the robot's current joint state and the joint angle range or preset distance corresponding to at least one singular pose. The control module 250 is further configured to: after switching to the second dragging mode, if it is determined that the robot is moving away from a singular pose, switch the current mode from the second dragging mode to the first dragging mode, and control the robot's movement in the first dragging mode. Whether to perform a mode switch is determined based on whether the robot is moving away from or approaching a singular pose, or based on whether the robot is moving away from or approaching a singular pose and the linear velocity and angular velocity of the robot's end effector.

[0201] In this embodiment, the virtual force calculation module 220 is further configured to: calculate a second virtual force to constrain the robot to reach the joint hard limit when the joint angle of the target joint is currently within the hard limit buffer zone. The control module 250 is specifically configured to: control the robot's movement in the first dragging mode based on the first virtual force, the second virtual force, and the target damping increase.

[0202] The control module 250 can also be used to: control the robot's movement in the first drag mode according to the second virtual force when the current position is not close to a singular pose but the joint angle of the target joint is in a hard limit buffer; and control the robot's movement 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 a preset force and the total torque is greater than a 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 within a hard limit buffer zone based on the current joint angle of the target joint, wherein the target joint does not include joints that can be in a singular pose and the corresponding rotation direction.

[0204] Optionally, the above modules can be stored in the form of software or firmware. Figure 1 The memory 110 shown is either stored in or embedded in the operating system (OS) of the electronic device 100, and can be used by... Figure 1 The processor 120 executes the program. Meanwhile, the data and program code required to execute the above modules can be stored in the memory 110.

[0205] This application also provides a readable storage medium storing a computer program thereon, which, when executed by a processor, implements the robot motion control method described above.

[0206] In summary, embodiments of this application provide a robot motion control method, device, electronic device, and readable storage medium. When the robot is currently in a first dragging mode based on admittance control, a first translational performance index and a first rotational performance index corresponding to the current joint angle are calculated. Then, when the robot is in a near-singular pose, a first virtual force is calculated based on the first translational and first rotational performance indices. Next, it is determined whether the total force magnitude and total torque magnitude of the first virtual force are greater than a preset force and a preset torque, respectively. If the total force magnitude or total torque magnitude of the first virtual force is not greater than the preset force or the preset torque magnitude is not greater than the preset torque, a target damping increase is calculated. Based on the first virtual force and the target damping increase, the robot's motion is controlled in the first dragging mode. Thus, by applying virtual forces and adjusting damping, the robot is prevented from being in a singular pose, while simultaneously reducing the degree of robot jitter that may be caused by speed changes.

[0207] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can also be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram and / or flowchart, and combinations of blocks in block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

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

[0209] If the aforementioned functions are implemented as software functional 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 this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0210] The above description is merely an optional embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A robot motion control method, characterized in that, The method comprises: Under the current first drag mode based on admittance control, calculate the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot. In the current near-singular pose, the first virtual force is calculated based on the first translation performance index and the first rotation performance index; Determine 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; If 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. The robot's movement is controlled in the first dragging mode based on the first virtual force and the target damping increase.

2. The method according to claim 1, characterized in that, The calculation of the first virtual force based on the first translational performance index and the first rotational performance index when the current near-singular pose condition includes: For each direction in Cartesian space, the first joint angle and the second joint angle at the next moment are calculated based on the unit forward and reverse velocity of that direction, the preset duration, and the current joint angle corresponding to that direction. Obtain a first performance index corresponding to the first joint angle and a second performance index corresponding to the second joint angle; Based on the first and second performance indicators corresponding to this direction, the gradient direction value corresponding to this direction is obtained; The first virtual force is calculated based on the gradient direction values ​​corresponding to each direction in 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 the angle corresponding to a unit positive velocity, and the second joint angle is the angle corresponding to a unit negative velocity. The step of obtaining the gradient direction value corresponding to the direction based on the first and second performance indicators includes: If 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 that direction is determined to be 0, wherein the corresponding value is the index that corresponds to the first translation performance index among the first translation performance index and the first rotation performance index. If the first difference between the first performance index and the corresponding value is not less than the second difference between the second performance index and the corresponding value, the gradient direction value in that direction is obtained based on the first difference. If the first difference is less than the second difference, the gradient direction value in that direction is obtained based on the second difference.

4. The method according to claim 1, characterized in that, The calculation yields the target damping increase, including: For each target dimension in the target damping increase, if the product of the force or torque corresponding to the target dimension in the first virtual force and the current velocity 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; 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. If the product of the force or torque corresponding to the target dimension in the first virtual force and the current velocity corresponding to the target dimension is less than 0, the component of the target dimension in the target damping increase is calculated based on the current velocity corresponding to the target dimension, wherein the larger the current velocity, the larger the component.

5. The method according to claim 1, characterized in that, The method further includes: Determine whether the first translation performance index is within the preset translation performance index range, wherein the minimum value of the preset translation performance index range is determined based on the translation performance index corresponding to the robot in different singular poses, and the maximum value of the preset translation performance index range is determined based on the translation performance index corresponding to the robot after moving from different singular poses in a direction away from the singular pose. Determine whether the first rotation performance index is within the preset rotation performance index range, wherein the minimum value of the preset rotation performance index range is determined based on the rotation performance index corresponding to the robot in different singular poses, and the maximum value of the preset rotation performance index range is determined based on the rotation performance index corresponding to the robot after moving from different singular poses in a direction away from the singular pose. If the first translation performance index is within the range of the preset translation performance index or the first rotation performance index is within the range of the preset rotation performance index, then the current near-singular pose is determined.

6. The method according to any one of claims 1-5, characterized in that, In the current admittance-based first drag mode, before calculating the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot, the method further includes: Determine whether the current singular pose handling strategy is avoidance or traversal; When the current singular pose handling strategy is to avoid, the steps of executing the first translational performance index and the first rotational performance index corresponding to the current joint angle of the computational robot are performed. When the current singular pose handling strategy is to traverse, if it is determined that the current position is close to a singular pose, 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 includes: Based on the robot's current joint state and at least one joint angle range or preset distance corresponding to a singular pose, determine whether the robot is currently close to a singular pose; and / or, After switching to the second drag mode, if it is determined that the robot is moving away from the singular pose, the current mode is switched from the second drag mode to the first drag mode, and the robot's movement is controlled in the first drag mode. Whether to switch modes is determined based on whether the robot is moving away from or moving towards the singular pose, or based on whether the robot is moving away from or moving towards the singular pose and the linear velocity and angular velocity of the robot's end effector.

8. The method according to any one of claims 1-5, characterized in that, The method further includes: When the joint angle of the target joint of the robot is currently in the hard limit buffer zone, a second virtual force is calculated to constrain the robot to reach the joint hard limit. When the current joint angle of the target joint is in a hard limit buffer zone but is not close to a singular pose, 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 in the first virtual force is greater than the preset force and the total torque is greater than the 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 step of controlling the robot's movement in the first dragging mode based on the first virtual force and the target damping increase includes: The robot's movement is controlled in the first dragging mode based on 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 includes: For each target joint of the robot, based on the current joint angle of the target joint, it is determined whether the current joint angle of the target joint is within the hard limit buffer zone. The target joint does not include joints that can be in a singular pose and the corresponding rotation direction. When the robot currently has a target joint angle within a hard limit buffer zone, the second virtual force used to constrain the robot to reach the joint hard limit is calculated, including: For each target joint of the robot, when the joint angle of the target joint is within the hard limit buffer, the virtual force of the joint space corresponding to the target joint is calculated based on the current joint angle of the target joint and the hard limit buffer it is in. The second virtual force is calculated based on the joint space virtual force corresponding to each target joint of the robot.

10. A robot motion control device, characterized in that, The device comprises: The index calculation module is used to calculate the first translational performance index and the first rotational performance index corresponding to the current joint angle of the robot when it is currently in the first drag mode based on admittance control. The virtual force calculation module is used to calculate the first virtual force based on the first translation performance index and the first rotation performance index when the current near-singular pose is present. The judgment module is used to determine 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; The damping adjustment calculation module is used to calculate the target damping increase when 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 control module is used to control the robot's movement in the first dragging mode based on the first virtual force and the target damping increase.

Images