Safety control method and system for robot collision

Abstract

The application provides a mechanical arm collision safety control method and system, the method comprises the following steps: recording the motion state data of each joint of the mechanical arm in real time, and storing the motion state data; detecting whether the mechanical arm collides; when the mechanical arm collides, obtaining the initial motion state of each joint of the mechanical arm at the time of collision according to the motion state data, and the expected path position of each joint after collision; obtaining the expected motion trajectory of each joint after collision according to the expected path position of each joint after collision; and carrying out impedance control on each joint according to the expected motion trajectory of each joint after collision, so that each joint retreats according to the expected motion trajectory after collision. The application can reduce the collision force in the collision process, effectively control the motion posture of each joint in the mechanical arm, reduce the risk of secondary collision, and ensure the safety of man-machine.

Description

Technical Field

[0001] This invention relates to the field of robotic arm automation control technology, specifically to a robotic arm collision safety control method and a robotic arm collision safety control system. Background Technology

[0002] When a robot collides with a person or the environment, most current control strategies involve first switching the robot to speed control mode to decelerate and stop, reducing the maximum collision force. After deceleration, the robot switches to zero-gravity control mode to control each joint of the robotic arm, further reducing the steady-state collision force. However, this post-collision control strategy has several problems. First, limited by the robot's deceleration capabilities, the robot's speed is high at the time of collision, resulting in a long deceleration distance and a large collision force, making it difficult to ensure human-robot safety. In practice, safety can only be ensured by reducing the robot's operating speed, which reduces production efficiency. Second, after decelerating and stopping, switching to zero-gravity mode causes the robot to move or tend to move in the opposite direction of the collision. The robot's subsequent movement depends entirely on the contact force during the sudden stop and its own dynamic characteristics. Because this movement is uncontrolled, it is easy for secondary collisions with people or the surrounding environment to occur, further complicating human-robot safety. Summary of the Invention

[0003] To address the aforementioned technical problems, this invention provides a method and system for safety control of a robotic arm after a collision. This method and system can reduce the collision force during the collision process, effectively control the movement posture of each joint in the robotic arm, reduce the risk of secondary collisions, and ensure human-machine safety.

[0004] The technical solution adopted in this invention is as follows:

[0005] A method for safety control of a robotic arm after a collision includes the following steps: recording and storing motion state data of each joint of the robotic arm in real time; detecting whether the robotic arm has collided; when the robotic arm collides, obtaining the initial motion state of each joint of the robotic arm at the time of the collision and the expected path position of each joint after the collision based on the motion state data, wherein the initial motion state includes initial joint position, initial joint velocity, and initial joint acceleration; obtaining the expected motion trajectory of each joint after the collision based on the expected path position of each joint after the collision; and performing impedance control on each joint based on the expected motion trajectory of each joint after the collision, so that each joint retracts according to the expected motion trajectory after the collision.

[0006] In addition, the robotic arm collision safety control method proposed above according to the present invention may also have the following additional technical features:

[0007] According to one embodiment of the present invention, the motion state data of each joint of the robotic arm is recorded and stored in real time within a preset time period, wherein the preset time period is the maximum time delay between the actual collision time of the robotic arm and the detection time of the collision, and the preset time period includes N sampling periods.

[0008] According to one embodiment of the present invention, the desired path position is the historical joint position from the k-th sampling period to the (k-N+1)-th sampling period when a collision is detected, where k is an integer greater than N.

[0009] According to one embodiment of the present invention, the step of impedance control of each joint based on the desired motion trajectory after each joint collision specifically includes: calculating an estimated value of the collision torque of each joint of the robotic arm under the desired motion trajectory; adjusting the stiffness parameter and damping parameter of the impedance control based on the estimated value of the collision torque, and performing impedance control on each joint.

[0010] According to one embodiment of the present invention, the stiffness parameter is adjusted based on an adjustment formula, the adjustment formula being as follows:

[0011]

[0012] Where K0 is the default stiffness parameter, (-τ0, τ0) is the collision force safety range, τ0 is the collision force safety threshold, and K(j) and Let α represent the estimated values ​​of stiffness parameter and collision moment in the j-th sampling period, and let α represent the coefficient of variation of stiffness parameter with collision force.

[0013] In addition, to achieve the above objectives, the present invention also proposes a safety control system for a robotic arm after a collision.

[0014] A safety control system for a robotic arm after a collision includes: a state memory for recording and storing motion state data of each joint of the robotic arm in real time; a collision detector for detecting whether the robotic arm has collided; a data extractor for obtaining, based on the motion state data, the initial motion state of each joint of the robotic arm at the time of the collision and the expected path position of each joint after the collision, wherein the initial motion state includes initial joint position, initial joint velocity, and initial joint acceleration; a trajectory calculator for calculating the expected motion trajectory of each joint after the collision based on the expected path position of each joint after the collision; and an impedance controller for performing impedance control on each joint based on the expected motion trajectory of each joint after the collision, so that each joint retracts according to the expected motion trajectory after the collision.

[0015] According to one embodiment of the present invention, the state memory is used to record and store the motion state data of each joint of the robotic arm in real time within a preset time, wherein the preset time is the maximum time delay between the actual moment of collision of the robotic arm and the moment of detection of collision of the robotic arm, and the preset time includes N sampling periods.

[0016] According to one embodiment of the present invention, the desired path position is the historical joint position from the k-th sampling period to the (k-N+1)-th sampling period when a collision is detected, where k is an integer greater than N.

[0017] According to one embodiment of the present invention, the impedance controller specifically includes: a calculation submodule, which is used to calculate an estimated value of the collision torque of each joint of the robotic arm under the desired motion trajectory; and an adjustment submodule, which is used to adjust the stiffness parameter and damping parameter of the impedance control according to the estimated value of the collision torque, and to perform impedance control on each joint.

[0018] According to one embodiment of the present invention, the stiffness parameter is adjusted based on an adjustment formula, the adjustment formula being as follows:

[0019]

[0020] Where K0 is the default stiffness parameter, (-τ0, τ0) is the collision force safety range, τ0 is the collision force safety threshold, and K(j) and Let α represent the estimated values ​​of stiffness parameter and collision moment in the j-th sampling period, and let α represent the coefficient of variation of stiffness parameter with collision force.

[0021] The beneficial effects of this invention are:

[0022] The robotic arm collision safety control method of this invention records and stores the motion state data of each joint of the robotic arm in real time. Therefore, when a collision is detected, the initial motion state of each joint of the robotic arm at the time of the collision, as well as the expected path position of each joint after the collision, can be obtained immediately, improving the robot's response speed and shortening the response time. Impedance control is performed to make each joint return to its initial joint position according to the expected path position after the collision, so that the robot has a certain degree of compliance with the collision force, reducing the collision force during the collision process, ensuring that the collision force is within a safe range, and effectively controlling the motion posture of each joint in the robotic arm, reducing the risk of secondary collisions and ensuring human-machine safety. Attached Figure Description

[0023] Figure 1 This is a flowchart of a robotic arm collision safety control method according to an embodiment of the present invention;

[0024] Figure 2 This is a block diagram of the safety control system for the robotic arm after a collision, according to an embodiment of the present invention. Detailed Implementation

[0025] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0026] like Figure 1 As shown, the robotic arm collision safety control method of this embodiment includes the following steps:

[0027] S1 records and stores the motion state data of each joint of the robotic arm in real time.

[0028] It should be noted that by recording and storing the motion state data of each joint of the robotic arm in real time, the initial motion state of each joint of the robotic arm at the time of the collision and the expected path position of each joint after the collision can be obtained immediately when a collision is detected, thereby improving the robot's response speed and shortening the response time.

[0029] Specifically, the motion state data of each joint of the robotic arm can be recorded and stored in real time within a preset time period. The preset time is the maximum time delay between the actual moment of collision and the moment of collision detection, and includes N sampling periods. The maximum time delay between the actual moment of collision and the moment of collision detection is the maximum error of the robot collision detection algorithm, denoted as N*Ts, where Ts represents the sampling period.

[0030] In one embodiment of the invention, each joint includes a driver, which can be used to record and store the motion state data of each joint of the robotic arm in real time over a preset time period. The robot's controller communicates with the driver of each joint via a bus, wherein the sampling frequency of the driver is higher than the bus communication frequency, thus enabling post-collision safety control within the driver of each joint to improve response speed.

[0031] The actuator records and stores the motion state data of each joint of the robotic arm in real time within a preset time, which not only ensures that the joint motion trajectory during the actual collision process is recorded and stored, but also saves actuator resources to the maximum extent.

[0032] S2, detect whether the robotic arm has collided.

[0033] In one embodiment of the present invention, the robot controller may rely on a robot collision detection algorithm to detect whether the robotic arm has collided and transmit the collision information to the driver.

[0034] S3, when the robotic arm collides, the initial motion state of each joint of the robotic arm at the time of the collision is obtained based on the motion state data, as well as the expected path position of each joint after the collision. The initial motion state includes the initial joint position, the initial joint velocity, and the initial joint acceleration.

[0035] In one embodiment of the present invention, if the robot controller detects a collision at time k*Ts, i.e., the driver detects a collision in the kth sampling period, then the data stored in the driver of each joint includes the historical joint positions {q(k), q(k-1), ..., q(k-N+1)} from the (k-N+1)th sampling period to the kth sampling period, and the motion state in the kth sampling period. The initial motion state of each joint of the robotic arm at the time of collision is the motion state of each joint in the k-th sampling period. The desired path position is the historical joint position {q(k), q(k-1), ..., q(k-N+1)} from the k-th sampling period to the (k-N+1)-th sampling period when a collision is detected, where k is an integer greater than N, the k-th sampling period is the k-th sampling period since the driver started recording motion state data, and q(k) is the joint position in the k-th sampling period. Let the joint velocity be the value in the k-th sampling period. Let be the joint acceleration in the kth sampling period.

[0036] S4: Based on the expected path position after each joint collision, obtain the expected motion trajectory after each joint collision.

[0037] In one embodiment of the present invention, the desired motion trajectory of each joint after a collision can be obtained by trajectory interpolation based on the desired path position of each joint after the collision.

[0038] S5, based on the desired motion trajectory of each joint after the collision, performs impedance control on each joint so that each joint retracts according to the desired motion trajectory after the collision.

[0039] In one embodiment of the present invention, step S5 specifically includes the following steps:

[0040] S51, calculate the estimated collision torque of each joint of the robotic arm under the desired motion trajectory.

[0041] In one embodiment of the present invention, the estimated value of the collision torque of each joint of the robotic arm during the collision process can be calculated based on the joint motor current and dynamic model.

[0042] The dynamic equations of an N-degree-of-freedom collaborative robot are:

[0043]

[0044] Where q∈Rn M(q) ∈ R represents joint position and joint velocity, respectively; n×n Represents the robot's inertia matrix. Represents the Coriolis force-centrifugal force matrix, G(q)∈R n τ represents the gravitational moment vector; m ∈R n τ represents the joint driving torque vector. ext ∈R n This represents the external torque vector of the joint, i.e., the collision torque vector.

[0045] The goal of impedance control is to control the dynamic response relationship between the control force and the motion state. The impedance control rate is shown in equation (2):

[0046]

[0047] Where, q r ∈R n×1 This indicates the joint reference position in the desired motion trajectory. This represents the model-based estimate of the gravitational moment vector. This represents the estimated frictional torque; K and D are impedance control parameters, representing the stiffness parameter and damping parameter, respectively.

[0048] Combining equations (1) and (2), we obtain the collision torque τ. ext With motion state Dynamic response between:

[0049]

[0050] Where E represents the compensation error for gravity and friction. Therefore, the joint exhibits second-order mass-damped-stiffness system characteristics, q r It is also the equilibrium position of the joint when it is not subjected to external forces. The equilibrium velocity of the joint when it is in the equilibrium position.

[0051] Estimated collision torque of each joint of the robotic arm during the collision. It can be calculated from equation (3).

[0052] As can be seen from equation (3), by adjusting K and D, the relationship between external force and joint deviation from equilibrium position can be adjusted, so that the robot can maintain compliance while maintaining the desired motion.

[0053] S52 adjusts the stiffness and damping parameters of the impedance control based on the estimated value of the collision moment, and performs impedance control on each joint.

[0054] In one embodiment of the present invention, the adjustment rule for the stiffness parameter K is as follows: assuming the default parameter of K is K0, during the post-collision safety control process, when the estimated value of the collision moment... If the collision force exceeds the safe collision force threshold, reduce the stiffness parameter K to reduce the collision force; when When the collision force returns to within the safe collision force threshold, K reverts to the default stiffness parameter K0 to improve position tracking accuracy. The safe collision force is selected based on collision safety standards and the collision force estimation error.

[0055] The stiffness parameter K can be adjusted using fuzzy rules or an adaptive method, and the damping parameter D can be adjusted based on equation (3) and the adjusted stiffness parameter K.

[0056] In one embodiment of the present invention, the stiffness parameter can be adjusted based on an adjustment formula, as follows:

[0057]

[0058] Where K0 is the default stiffness parameter, (-τ0, τ0) is the collision force safety range, τ0 is the collision force safety threshold, and K(j) and Let α represent the estimated values ​​of stiffness parameter and collision moment in the j-th sampling period, and let α represent the coefficient of variation of stiffness parameter with collision force.

[0059] The robotic arm collision safety control method according to embodiments of the present invention records and stores the motion state data of each joint of the robotic arm in real time. Therefore, when a collision is detected, the initial motion state of each joint of the robotic arm at the time of the collision, as well as the expected path position of each joint after the collision, can be obtained immediately, improving the robot's response speed and shortening the response time. Impedance control is performed to make each joint return to its initial joint position according to the expected path position after the collision, so that the robot has a certain degree of compliance with the collision force, reducing the collision force during the collision process, ensuring that the collision force is within a safe range, and effectively controlling the motion posture of each joint in the robotic arm, reducing the risk of secondary collisions and ensuring human-machine safety.

[0060] Corresponding to the robotic arm collision safety control method in the above embodiments, the present invention also proposes a robotic arm collision safety control system.

[0061] like Figure 2 As shown, the robotic arm collision safety control system of this embodiment includes: a state memory 10, a collision detector 20, a data extractor 30, a trajectory calculator 40, and an impedance controller 50. The state memory 10 records and stores the motion state data of each joint of the robotic arm in real time. The collision detector 20 detects whether a collision has occurred. When a collision occurs, the data extractor 30 obtains the initial motion state of each joint of the robotic arm at the time of the collision, and the expected path position of each joint after the collision, based on the motion state data. The initial motion state includes the initial joint position, initial joint velocity, and initial joint acceleration. The trajectory calculator 40 calculates the expected motion trajectory of each joint after the collision based on the expected path position of each joint. The impedance controller 50 performs impedance control on each joint based on the expected motion trajectory after the collision, so that each joint retracts according to the expected motion trajectory after the collision.

[0062] It should be noted that the state memory 10 records and stores the motion state data of each joint of the robotic arm in real time. Therefore, when a collision is detected, the initial motion state of each joint of the robotic arm at the time of the collision, as well as the expected path position of each joint after the collision, can be obtained immediately, thereby improving the robot's response speed and shortening the response time.

[0063] Specifically, the state memory 10 can record and store the motion state data of each joint of the robotic arm in real time within a preset time. The preset time is the maximum time delay between the actual moment of collision and the moment of collision detection, and includes N sampling periods. The maximum time delay between the actual moment of collision and the moment of collision detection is the maximum error of the robot collision detection algorithm, denoted as N*Ts, where Ts represents the sampling period.

[0064] In one embodiment of the present invention, each joint includes a driver, and a state memory 10 may be disposed within the driver. Therefore, the driver can be used to record and store the motion state data of each joint of the robotic arm in real time within a preset time. The robot's controller communicates with the driver of each joint via a bus. The sampling frequency of the driver is higher than the bus communication frequency, so safety control after a collision can be performed within the driver of each joint to improve response speed.

[0065] The actuator records and stores the motion state data of each joint of the robotic arm in real time within a preset time, which not only ensures that the joint motion trajectory during the actual collision process is recorded and stored, but also saves actuator resources to the maximum extent.

[0066] In one embodiment of the present invention, the collision detector 20 is disposed in the robot controller, and the data extractor 30 may be disposed in the driver. The collision detector 20 of the robot controller may rely on the robot collision detection algorithm to detect whether the robotic arm has collided and transmit the collision information to the driver.

[0067] In one embodiment of the present invention, if the robot controller detects a collision at time k*Ts, i.e., the driver detects a collision in the kth sampling period, then the data stored in the driver of each joint includes the historical joint positions {q(k), q(k-1), ..., q(k-N+1)} from the (k-N+1)th sampling period to the kth sampling period, and the motion state in the kth sampling period. The initial motion state of each joint of the robotic arm at the time of collision is the motion state of each joint in the k-th sampling period. The desired path position is the historical joint position {q(k), q(k-1), ..., q(k-N+1)} from the k-th sampling period to the (k-N+1)-th sampling period when a collision is detected, where k is an integer greater than N, the k-th sampling period is the k-th sampling period since the driver started recording motion state data, and q(k) is the joint position in the k-th sampling period. Let the joint velocity be the value in the k-th sampling period. Let be the joint acceleration in the kth sampling period.

[0068] In one embodiment of the present invention, the trajectory calculator 40 can obtain the expected motion trajectory of each joint after the collision by trajectory interpolation based on the expected path position of each joint after the collision.

[0069] In one embodiment of the present invention, the impedance controller 50 specifically includes a calculation submodule and an adjustment submodule, wherein the calculation submodule is used to calculate the estimated value of the collision torque of each joint of the robotic arm under the desired motion trajectory; the adjustment submodule is used to adjust the stiffness parameter and damping parameter of the impedance control according to the estimated value of the collision torque and the joint trajectory tracking error, and to perform impedance control on each joint.

[0070] In one embodiment of the present invention, the calculation submodule can calculate the estimated value of the collision torque of each joint of the robotic arm during the collision process based on the joint motor current and dynamic model.

[0071] The goal of impedance control is to control the dynamic response relationship between the control force and the motion state. Combining equations (1) and (2), the collision torque τ is obtained. ext With motion state The dynamic response between them. Estimated values ​​of the collision torque at each joint of the robotic arm during the collision. It can be calculated by equation (3). As can be seen from equation (3), by adjusting K and D, the relationship between the external force and the deviation between the joint and the equilibrium position can be adjusted, so that the robot can be compliant while maintaining the desired motion.

[0072] In one embodiment of the present invention, the adjustment rule for the stiffness parameter K is as follows: assuming the default parameter of K is K0, during the post-collision safety control process, when the estimated value of the collision moment... If the collision force exceeds the safe collision force threshold, reduce the stiffness parameter K to reduce the collision force; when When the collision force returns to within the safe collision force threshold, K reverts to the default stiffness parameter K0 to improve position tracking accuracy. The safe collision force is selected based on collision safety standards and the collision force estimation error.

[0073] The stiffness parameter K can be adjusted using fuzzy rules or an adaptive method, and the damping parameter D can be adjusted based on equation (3) and the adjusted stiffness parameter K.

[0074] In one embodiment of the present invention, the adjustment submodule can adjust the stiffness parameter based on the adjustment formula of the stiffness parameter, as follows:

[0075]

[0076] Where K0 is the default stiffness parameter, (-τ0, τ0) is the collision force safety range, τ0 is the collision force safety threshold, and K(j) and Let α represent the estimated values ​​of stiffness parameter and collision moment in the j-th sampling period, and let α represent the coefficient of variation of stiffness parameter with collision force.

[0077] The robotic arm collision safety control system according to embodiments of the present invention records and stores the motion state data of each joint of the robotic arm in real time. Therefore, when a collision is detected, the initial motion state of each joint of the robotic arm at the time of the collision, as well as the expected path position of each joint after the collision, can be obtained immediately, thereby improving the robot's response speed and shortening the response time. Impedance control is performed to make each joint return to its initial joint position according to the expected path position after the collision, so that the robot has a certain degree of compliance with the collision force, reducing the collision force during the collision process, ensuring that the collision force is within a safe range, and effectively controlling the motion posture of each joint in the robotic arm, reducing the risk of secondary collisions and ensuring human-machine safety.

[0078] In the description of this invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. "A plurality of" means two or more, unless otherwise explicitly specified.

[0079] In this invention, unless otherwise explicitly specified and limited, the terms "installation," "connection," "linking," and "fixing," etc., should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0080] In this invention, unless otherwise explicitly specified and limited, "above" or "below" the second feature can mean that the first feature is in direct contact with the second feature, or that the first feature is in indirect contact with the second feature through an intermediate medium. Furthermore, "above," "over," and "on top" of the second feature can mean that the first feature is directly above or diagonally above the second feature, or simply that the first feature is at a higher horizontal level than the second feature. "Below," "below," and "under" the second feature can mean that the first feature is directly below or diagonally below the second feature, or simply that the first feature is at a lower horizontal level than the second feature.

[0081] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Furthermore, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0082] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing a particular logical function or process, and the scope of the preferred embodiments of the invention includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as will be understood by those skilled in the art to which embodiments of the invention pertain.

[0083] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0084] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0085] Those skilled in the art will understand that all or part of the steps of the methods described in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it includes one or a combination of the steps of the method embodiments.

[0086] Furthermore, the functional units in the various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0087] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A safety control method for a robotic arm after a collision, characterized in that, Includes the following steps: The motion state data of each joint of the robotic arm is recorded in real time within a preset time period and stored. The preset time period is the maximum time delay between the actual moment of collision of the robotic arm and the moment of detection of collision of the robotic arm. The preset time period includes N sampling periods. Detect whether the robotic arm has collided; When the robotic arm collides, the initial motion state of each joint of the robotic arm at the time of the collision and the expected path position of each joint after the collision are obtained based on the motion state data. The initial motion state includes the initial joint position, the initial joint velocity and the initial joint acceleration. Based on the expected path position after each joint collision, the expected motion trajectory after each joint collision is obtained. The expected path position is the historical joint position from the k-th sampling period to the (k-N+1)-th sampling period when the collision is detected, where k is an integer greater than N. Impedance control is applied to each joint based on the desired motion trajectory after the collision, so that each joint retracts according to the desired motion trajectory after the collision. Specifically, this includes: calculating an estimated value of the collision torque of each joint of the robotic arm under the desired motion trajectory; adjusting the stiffness and damping parameters of the impedance control based on the estimated collision torque; and adjusting the stiffness parameters based on the adjustment formula for the stiffness parameters, as follows: in, The default stiffness parameters are... Within the safe range of collision force, The collision force safety threshold, and Indicates the first j Estimates of stiffness parameters and collision moments during the sampling period. This represents the coefficient of variation of the stiffness parameter with the impact force.

2. A safety control system for a robotic arm after a collision, characterized in that, include: A state memory is used to record the motion state data of each joint of the robotic arm in real time within a preset time, and to store the motion state data. The preset time is the maximum time delay between the actual moment of collision of the robotic arm and the moment of detection of collision of the robotic arm. The preset time includes N sampling periods. A collision detector, used to detect whether the robotic arm has collided with something; The data extractor, when the robotic arm collides, obtains the initial motion state of each joint of the robotic arm at the time of the collision and the expected path position of each joint after the collision based on the motion state data, wherein the initial motion state includes the initial joint position, the initial joint velocity and the initial joint acceleration; A trajectory calculator is used to calculate the expected motion trajectory of each joint after a collision based on the expected path position after each collision. The expected path position is the historical joint position from the k-th sampling period to the (k-N+1)-th sampling period when the collision is detected, where k is an integer greater than N. An impedance controller is provided to perform impedance control on each joint based on the desired motion trajectory after a collision, so that each joint retracts according to the desired motion trajectory after the collision. The impedance controller specifically includes: a calculation submodule for calculating an estimated value of the collision torque of each joint of the robotic arm under the desired motion trajectory; and an adjustment submodule for adjusting the stiffness and damping parameters of the impedance control based on the estimated collision torque, thereby performing impedance control on each joint. The adjustment submodule adjusts the stiffness parameters based on an adjustment formula for the stiffness parameters, the formula being as follows: in, The default stiffness parameters are... Within the safe range of collision force, The collision force safety threshold, and Indicates the first j Estimates of stiffness parameters and collision moments during the sampling period. This represents the coefficient of variation of the stiffness parameter with the impact force.

Images