Actuator control method of hand-push type robot

Abstract

The invention relates to an actuator control method of a hand-push type robot, and belongs to the technical field of program control manipulators. According to the method, coordinate systems are established through a base of the robot and a six-dimensional force sensor respectively, then the load barycentric coordinates and the load gravity of an actuator are solved through at least three times of pose transformation, and then the load gravity component and the load gravity torque component of the load gravity on all axes of the sensor coordinate systems are calculated; further calculating the applied force and the applied torque to the actuator at different moments, and solving the acceleration and the speed when the actuator is pushed to move and rotate according to the applied force and the applied torque; and finally, obtaining a coefficient according to the moving distance range and the rotating angle range of the actuator, and calculating the safety displacement amount and the safety angle amount of the actuator according to the coefficient, the speed and the reading interval time. When force is applied to enable the actuator to reach a safe amount, the actuator is locked. According to the method, the acceleration, the speed and the safety range during actuator control are accurate, the control requirements of people can be met, and the control safety is effectively improved.

Description

Technical Field

[0001] The invention relates to an actuator control method of a hand-push robot, belonging to the technical field of program-controlled manipulators (B25J9 / 00). Background Art

[0002] With the continuous development and maturity of industrial robots and related technologies, as well as the continuous increase in labor costs, the role of robots in production has become increasingly important. Today, many factories have begun to use robots and humans to interact with each other to improve work efficiency and assembly accuracy. However, the control systems of traditional industrial robots are usually bulky and require two-handed operation. At least two operators are required to simultaneously perform force control operations on the actuators and adjust the robot's running speed. This not only places high demands on the collaboration between operators, but also in a noisy factory environment, the communication efficiency between personnel is often significantly reduced, which affects production efficiency and even increases the risk of safety accidents.

[0003] The traditional control method of hand-push robots is to install pressure sensors on the surface of the robot arm, and the robot will stop working only when the contact pressure exceeds the set value. However, due to the influence of inertia, this method cannot cope with high-speed and high-precision production tasks, especially in the scenario of manual manual pushing, which often poses a great safety hazard.

[0004] Therefore, a more intelligent and precise control method for push robot actuators is urgently needed to reduce the safety risks in the interactive operation between push robots and humans. This is not only to ensure the safety of operators, but also a necessary condition to ensure the smooth and efficient operation of the production process. Summary of the invention

[0005] The technical problem to be solved by the present invention is: how to improve the safety of a hand-push robot during operation.

[0006] The technical solution proposed by the present invention to solve the above technical problems is: a method for controlling an actuator of a hand-push robot, wherein the actuator and the six-dimensional force sensor are installed at the end of the robot, comprising the following steps:

[0007] Step 1: Establish a base coordinate system with the base of the robot as the origin, and establish a sensor coordinate system with the six-dimensional force sensor as the origin;

[0008] Step 2: Control the actuator to perform at least three posture changes in the loaded state with the workpiece picked up and the unloaded state without the workpiece picked up, and calculate the load center of gravity coordinates of the actuator in the sensor coordinate system according to the following formula (1):

[0009] (1);

[0010] In formula (1), … is the first x-axis load gravity moment component of the actuator from the first pose to the nth pose on the x-axis of the sensor coordinate system; … is the first y-axis load gravity moment component of the actuator from the first pose to the nth pose on the y-axis of the sensor coordinate system; … is the first z-axis load gravity moment component of the actuator from the first pose to the nth pose on the z-axis of the sensor coordinate system; , , They are respectively the sensor coordinate system when the actuator is not subject to external force , , The x-axis no-load gravity moment component, the y-axis no-load gravity moment component and the z-axis no-load gravity moment component on the axis; … is the first x-axis load gravity component of the actuator from the first pose to the nth pose on the x-axis of the sensor coordinate system; … is the first y-axis load gravity component of the actuator from the first pose to the nth pose on the y-axis of the sensor coordinate system; … is the first z-axis load gravity component of the actuator from the first pose to the nth pose on the z-axis of the sensor coordinate system;

[0011] Step 2.1: Calculate the actuator load gravity according to formula (2)

[0012] (2);

[0013] In formula (2), … is the first transformation matrix between the sensor coordinate system and the base coordinate system from the first pose to the nth pose; , , They are respectively the x-axis no-load gravity component, the y-axis no-load gravity component and the z-axis no-load gravity component on the x, y and z-axes of the sensor coordinate system when the actuator is not subjected to external force; , , are respectively a second x-axis load gravity component, a second y-axis load gravity component, and a second z-axis load gravity component of the actuator on the x, y, and z axes of the base coordinate system; is the third-order identity matrix;

[0014] Step 2.2: Calculate the load gravity according to formula (3) The third x-axis load gravity component on the x, y, z axes of the sensor coordinate system , the third y-axis load gravity component and the third z-axis load gravity component

[0015] (3);

[0016] In formula (3), is the second transformation matrix between the sensor coordinate system and the base coordinate system; T is the transpose;

[0017] Step 2.3: Calculate the load gravity according to formula (4) The second x-axis load gravity moment component on the x, y, z axes of the sensor coordinate system , the second y-axis load gravity moment component and the second z-axis load gravity moment component

[0018] (4);

[0019] Step 3: Set the interval time Q for reading the six-dimensional force sensor readings; the time when the six-dimensional force sensor readings are read for the first time is taken as time t1, and the time when the six-dimensional force sensor readings are read for the second time after the interval time Q is taken as time t2, and so on until time tn;

[0020] Push the actuator to perform workpiece assembly operations and perform the following steps;

[0021] Step 3.1: Read the time t1 in the sensor coordinate system , , The x-axis total force component on the axis , total force component on the y-axis and the z-axis total force component , and in the sensor coordinate system , , The x-axis total moment component on the axis , total moment component on the y-axis and the total moment component in the z-axis ;

[0022] Step 3.2: Calculate the x-axis force component of the force applied to the actuator at time t1 on the x, y, and z axes of the sensor coordinate system according to equation (5): , y-axis applied force component and the z-axis force component , and the torque applied to the actuator at time t1 is the torque component applied to the x-axis on the x, y, and z axes of the sensor coordinate system , the torque component applied on the y-axis and the z-axis applied moment component

[0023] (5);

[0024] Step 3.3: Calculate the x-axis acceleration of the actuator moving along the x, y, and z axes of the sensor coordinate system at time t1 using equation (6): , y-axis moving acceleration , z-axis moving acceleration , x-axis moving speed , y-axis moving speed and z-axis moving speed

[0025] (6);

[0026] In formula (6), m is the total mass of the actuator and the workpiece; is the virtual damping, ; is the moving speed at the moment before t1. For the first reading of the six-dimensional force sensor at t1, the moving speed at the moment before t1 is , , All are 0;

[0027] The x-axis angular acceleration of the actuator rotating around the x, y, and z axes of the sensor coordinate system at time t1 is calculated using formula (7): , y-axis angular acceleration 、z-axis angular acceleration , x-axis angular velocity , y-axis angular velocity and the z-axis angular velocity

[0028] (7);

[0029] In formula (7), j is the moment of inertia of the actuator; is the angular velocity at the moment before t1. For the first reading of the six-dimensional force sensor at t1, the angular velocity at the moment before t1 is , , All are 0;

[0030] Step 4: Determine the x-axis distance range of the actuator moving along the x, y, and z axes of the sensor coordinate system according to the on-site working conditions , y-axis distance range And z-axis distance range , and the x-axis angle range of the actuator rotating around the x, y, and z axes of the sensor coordinate system Y-axis angle range and z-axis angle range ;

[0031] The x-axis movement coefficient of the actuator moving along the x, y, and z axes of the sensor coordinate system is calculated according to the following formula (8): , y-axis movement coefficient and z-axis movement coefficient

[0032] (8);

[0033] In formula (8), is the first safety estimation threshold for the actuator to move along the x, y, and z axes of the sensor coordinate system, respectively, and is determined based on experience. ; , and are the x-axis initial movement coefficient, the y-axis initial movement coefficient and the z-axis initial movement coefficient on the x, y and z-axes of the sensor coordinate system respectively;

[0034] The x-axis rotation coefficient of the actuator rotating around the x, y, and z axes of the sensor coordinate system is calculated by the following formula (9): , y-axis rotation coefficient and the z-axis rotation coefficient

[0035] (9);

[0036] In formula (9), , and are the x-axis angle, y-axis angle and z-axis angle of the actuator rotating around the x, y and z axes of the sensor coordinate system; is the second safety estimation threshold for the actuator to rotate around the x, y, and z axes of the sensor coordinate system, respectively, and is determined based on experience. ; , and are respectively the x-axis initial rotation coefficient, the y-axis initial rotation coefficient and the z-axis initial rotation coefficient on the x, y and z-axes of the sensor coordinate system;

[0037] Step 5: Calculate the first x-axis safe displacement of the actuator along the x, y, and z axes of the sensor coordinate system at time t1 according to formula (10): , the first y-axis safety displacement and the first z-axis safety displacement , and the first x-axis safety angle of the actuator rotating around the x, y, and z axes of the sensor coordinate system at time t1 , the first y-axis safety angle and the first z-axis safety angle

[0038] (10);

[0039] Step 6: reading in real time a first x-axis displacement, a first y-axis displacement, and a first z-axis displacement of the actuator along the x, y, and z axes of the sensor coordinate system caused by the force applied to the actuator during the process from time t1 to time t2, as well as a first x-axis angle value, a first y-axis angle value, and a first z-axis angle value of the actuator rotating around the x, y, and z axes of the sensor coordinate system;

[0040] If the first x-axis displacement, the first y-axis displacement and the first z-axis displacement reach the corresponding first x-axis safety displacement, the first y-axis safety displacement and the first z-axis safety displacement, or the first x-axis angle, the first y-axis angle and the first z-axis angle reach the corresponding first x-axis safety angle, the first y-axis safety angle and the first z-axis safety angle, the actuator stops suddenly and cannot be moved or rotated;

[0041] If the first x-axis displacement, the first y-axis displacement, and the first z-axis displacement do not reach the corresponding first x-axis safety displacement, the first y-axis safety displacement, and the first z-axis safety displacement, or the first x-axis angle, the first y-axis angle, and the first z-axis angle do not reach the corresponding first x-axis safety angle, the first y-axis safety angle, and the first z-axis safety angle, the assembly operation continues;

[0042] Step 7: When the assembly operation is performed to time t2, repeat steps 3-5 to calculate the second x-axis safety displacement, the second y-axis safety displacement and the second z-axis safety displacement of the actuator moving along the x, y and z axes of the sensor coordinate system at time t2, and the second x-axis safety angle, the second y-axis safety angle and the second z-axis safety angle of the actuator rotating around the x, y and z axes of the sensor coordinate system at time t2;

[0043] Read in real time the second x-axis displacement, the second y-axis displacement, and the second z-axis displacement of the actuator along the x, y, and z axes of the sensor coordinate system caused by the force applied to the actuator during the process from time t2 to time t3, as well as the second x-axis angle value, the second y-axis angle value, and the second z-axis angle value of the actuator rotating around the x, y, and z axes of the sensor coordinate system;

[0044] If the second x-axis displacement, the second y-axis displacement and the second z-axis displacement respectively reach the corresponding second x-axis safety displacement, the second y-axis safety displacement and the second z-axis safety displacement, or the second x-axis angle value, the second y-axis angle value and the second z-axis angle value respectively reach the corresponding second x-axis safety angle value, the second y-axis safety angle value and the second z-axis safety angle value, the actuator stops suddenly and cannot be moved or rotated;

[0045] If the second x-axis displacement, the second y-axis displacement, and the second z-axis displacement do not reach the corresponding second x-axis safety displacement, the second y-axis safety displacement, and the second z-axis safety displacement, or the second x-axis angle, the second y-axis angle, and the second z-axis angle do not reach the corresponding second x-axis safety angle, the second y-axis safety angle, and the second z-axis safety angle, the assembly operation continues;

[0046] And so on, until the actuator is emergency stopped or the assembly operation is completed.

[0047] Furthermore, the x, y, z axis distance ranges and x, y, z angle ranges determined in step 4 according to the on-site working conditions are determined using a visual calibration method.

[0048] Furthermore, the virtual damping in step 3.3 Adjustable, By increasing the value, the acceleration and speed of the actuator moving along the sensor coordinate system x, y, and z under the same applied force are controlled to be faster.

[0049] The beneficial effect of the present invention is that since the workpiece assembly operation is divided into sequential moments, the precise force component and torque component applied to the actuator at each moment are calculated. Based on these data, the movement acceleration and movement speed of the actuator along each of the three axes x, y, and z of the sensor coordinate system at each moment, as well as the angular acceleration and angular velocity around each of the three axes x, y, and z of the sensor coordinate system are further calculated, so that the movement of the actuator is more closely matched with the control during hand pushing. Finally, on this basis, the safety displacement and safety angle are calculated and set to prevent safety risks caused by excessive force. Overall, the dual setting of precise fitting control and safety quantity greatly improves the safety of the hand-pushing actuator during operation. DETAILED DESCRIPTION

[0050] Example

[0051] In this embodiment, an actuator control method of a push-type robot is provided, wherein the actuator and the six-dimensional force sensor are installed at the end of the robot, and the steps are as follows:

[0052] Step 1: Establish a base coordinate system with the robot base as the origin, and establish a sensor coordinate system with the six-dimensional force sensor as the origin;

[0053] The six-dimensional force sensor used in this embodiment is widely used in the field of robots and automation equipment, and can simultaneously measure forces in three directions and moments in three directions, thereby providing comprehensive force information.

[0054] Step 2: Control the actuator to perform at least three posture changes in the loaded state with the workpiece picked up and the unloaded state without the workpiece picked up, and calculate the load center of gravity coordinates of the actuator in the sensor coordinate system according to the following formula (1):

[0055] (1);

[0056] In formula (1), … is the first x-axis load gravity moment component of the actuator on the x-axis of the sensor coordinate system from the first pose to the nth pose; … is the first y-axis load gravity moment component of the actuator on the y-axis of the sensor coordinate system from the first pose to the nth pose; … is the first z-axis load gravity moment component of the actuator on the z-axis of the sensor coordinate system from the first pose to the nth pose; , , They are respectively the coordinates of the actuator in the sensor coordinate system when no external force is applied , , The x-axis no-load gravity moment component, the y-axis no-load gravity moment component and the z-axis no-load gravity moment component on the axis; … is the first x-axis load gravity component of the actuator on the x-axis of the sensor coordinate system from the first pose to the nth pose; … is the first y-axis load gravity component of the actuator on the y-axis of the sensor coordinate system from the first pose to the nth pose; … is the first z-axis load gravity component of the actuator on the z-axis of the sensor coordinate system from the first pose to the nth pose;

[0057] In this embodiment, the load center of gravity coordinates calculated by formula (1) are (50mm, 0, 100mm);

[0058] Step 2.1: Calculate the actuator load gravity according to formula (2)

[0059] (2);

[0060] In formula (2), … is the first transformation matrix between the sensor coordinate system and the base coordinate system from the first pose to the nth pose; , , They are the x-axis no-load gravity component, the y-axis no-load gravity component and the z-axis no-load gravity component on the x, y and z axes of the sensor coordinate system when the actuator is not subjected to external force; , , They are the second x-axis load gravity component, the second y-axis load gravity component and the second z-axis load gravity component of the actuator on the x, y and z axes of the base coordinate system; is the third-order identity matrix;

[0061] The load gravity calculated by formula (2) in this embodiment is =98 ;

[0062] Step 2.2: Calculate the load gravity according to formula (3) The third x-axis load gravity component on the sensor coordinate system x, y, z axes , the third y-axis load gravity component and the third z-axis load gravity component

[0063] (3);

[0064] In formula (3), is the second transformation matrix between the sensor coordinate system and the base coordinate system; T is the transpose;

[0065] In this embodiment, the formula (3) is used to calculate =0, =0, =98 ;

[0066] Step 2.3: Calculate the load gravity according to formula (4) The second x-axis load gravity moment component on the sensor coordinate system x, y, z axes , the second y-axis load gravity moment component and the second z-axis load gravity moment component

[0067] (4);

[0068] In this embodiment, the formula (4) is used to calculate =0, =4.9 , =0.

[0069] Step 3: Set the interval time Q for reading the six-dimensional force sensor to 4ms; the time when the six-dimensional force sensor is read for the first time is taken as time t1, and the time when the six-dimensional force sensor is read for the second time after the interval time Q is taken as time t2, and so on until time tn;

[0070] Push the actuator to perform workpiece assembly operations and perform the following steps;

[0071] Step 3.1: Read the sensor coordinate system at time t1 , , The x-axis total force component on the axis , total force component on the y-axis and the z-axis total force component , and in the sensor coordinate system , , The x-axis total moment component on the axis , total moment component on the y-axis and the total moment component in the z-axis ;

[0072] In this embodiment, read the time t1 =250 , =250 , =348 , = , = , = ;

[0073] Step 3.2: Calculate the x-axis force component of the force applied to the actuator at time t1 on the x, y, and z axes of the sensor coordinate system according to equation (5): , y-axis applied force component and the z-axis force component , and the torque component of the x-axis applied to the actuator at time t1 on the x, y, and z axes of the sensor coordinate system , the torque component applied on the y-axis and the z-axis applied moment component

[0074] (5);

[0075] In this embodiment, the calculated value is obtained according to formula (5): = , = , = , = , = , = ;

[0076] Step 3.3: Calculate the x-axis acceleration of the actuator moving along the x, y, and z axes of the sensor coordinate system at time t1 using equation (6): , y-axis moving acceleration , z-axis moving acceleration , x-axis moving speed , y-axis moving speed and z-axis moving speed

[0077] (6);

[0078] In formula (6), m is the total mass of the actuator and the workpiece; is the virtual damping, ; is the moving speed at the moment before t1. For the first reading of the six-dimensional force sensor at t1, the moving speed at the moment before t1 is , , All are 0;

[0079] In this embodiment, the total mass of the actuator and the workpiece is m= ; Pick , It can be adjusted according to specific working conditions. If the value is increased, the acceleration and speed of the actuator moving along the x, y, and z axes of the sensor coordinate system can be adjusted to increase under the same applied force. It can also cause the angular acceleration and angular speed of the actuator rotating around the x, y, and z axes of the sensor coordinate system to increase under the same applied force, which helps to save effort when pushing by hand. However, when the distance between the workpiece and the assembly object is close and high-precision operation is required, it should be increased. Turn down;

[0080] In this embodiment, the formula (6) is used to calculate =25 , =25 , =25 , =0.1 , =0.1 , =0.1 ;

[0081] According to the following formula (7), the x-axis angular acceleration of the actuator rotating around the x, y, and z axes of the sensor coordinate system at time t1 is calculated respectively: , y-axis angular acceleration 、z-axis angular acceleration , x-axis angular velocity , y-axis angular velocity and the z-axis angular velocity

[0082] (7);

[0083] In formula (7), j is the moment of inertia of the actuator; is the angular velocity at the moment before t1. For the first reading of the six-dimensional force sensor at t1, the angular velocity at the moment before t1 is , , All are 0;

[0084] In this embodiment, the formula (6) is used to calculate = , = , = , = , = , = .

[0085] Step 4: Determine the x-axis distance range of the actuator along the x, y, and z axes of the sensor coordinate system according to the on-site working conditions = , y-axis distance range = And z-axis distance range = , and the x-axis angle range of the actuator rotating around the x, y, and z axes of the sensor coordinate system = , y-axis angle range = and z-axis angle range = ;

[0086] In this embodiment, a visual calibration method widely used in the field of robot vision is used to determine the spatial envelope range of the actuator when loaded. The spatial envelope range is composed of the distance range of the actuator moving along the x, y, and z axes of the sensor coordinate system and the angle range of the actuator rotating around the x, y, and z axes of the sensor coordinate system.

[0087] The x-axis movement coefficient of the actuator moving along the x, y, and z axes of the sensor coordinate system is calculated using the following formula (8): , y-axis movement coefficient and z-axis movement coefficient

[0088] (8);

[0089] In formula (8), is the first safety estimation threshold for the actuator to move along the x, y, and z axes of the sensor coordinate system, respectively, and is determined based on experience. ; , and They are the x-axis initial movement coefficient, y-axis initial movement coefficient, and z-axis initial movement coefficient on the x, y, and z axes of the sensor coordinate system respectively;

[0090] In this embodiment, the first safety estimation threshold Set as , calculated according to formula (8) =1, =1, =1;

[0091] The x-axis rotation coefficient of the actuator rotating around the x, y, and z axes of the sensor coordinate system is calculated using the following formula (9): , y-axis rotation coefficient and the z-axis rotation coefficient

[0092] (9);

[0093] In formula (9), , and are the x-axis angle, y-axis angle, and z-axis angle of the actuator rotating around the x, y, and z axes of the sensor coordinate system; is the second safety estimation threshold for the actuator to rotate around the x, y, and z axes of the sensor coordinate system, respectively, and is determined based on experience. ; , and They are the initial x-axis rotation coefficient, the initial y-axis rotation coefficient, and the initial z-axis rotation coefficient on the x, y, and z axes of the sensor coordinate system respectively;

[0094] In this embodiment, the second safety estimation threshold Set as , calculated according to formula (9) =1, =1, =1.

[0095] Step 5: Calculate the first x-axis safe displacement of the actuator along the x, y, and z axes of the sensor coordinate system at time t1 using equation (10): , the first y-axis safety displacement and the first z-axis safety displacement , and the first x-axis safety angle of the actuator rotating around the sensor coordinate system x, y, and z axes at time t1 , the first y-axis safety angle and the first z-axis safety angle

[0096] (10);

[0097] In this embodiment, the formula (10) is used to calculate = , = , = , = , = , = .

[0098] Step 6: reading in real time the first x-axis displacement, the first y-axis displacement, and the first z-axis displacement of the actuator along the x, y, and z axes of the sensor coordinate system caused by the force applied to the actuator during the process from time t1 to time t2, as well as the first x-axis angle value, the first y-axis angle value, and the first z-axis angle value of the actuator rotating around the x, y, and z axes of the sensor coordinate system;

[0099] If the first x-axis displacement, the first y-axis displacement and the first z-axis displacement reach the corresponding first x-axis safety displacement, the first y-axis safety displacement and the first z-axis safety displacement, or the first x-axis angle, the first y-axis angle and the first z-axis angle reach the corresponding first x-axis safety angle, the first y-axis safety angle and the first z-axis safety angle, the actuator stops suddenly and cannot be moved or rotated;

[0100] If the first x-axis displacement, the first y-axis displacement, and the first z-axis displacement do not reach the corresponding first x-axis safety displacement, the first y-axis safety displacement, and the first z-axis safety displacement, or the first x-axis angle, the first y-axis angle, and the first z-axis angle do not reach the corresponding first x-axis safety angle, the first y-axis safety angle, and the first z-axis safety angle, the assembly operation continues;

[0101] In the process from time t1 to time t2 of this embodiment, the first x-axis displacement, the first y-axis displacement and the first z-axis displacement read in real time all fail to reach the corresponding first x-axis safety displacement, the first y-axis safety displacement and the first z-axis safety displacement, and the first x-axis angle value, the first y-axis angle value and the first z-axis angle value all fail to reach the corresponding first x-axis safety angle value, the first y-axis safety angle value and the first z-axis safety angle value, so the assembly operation continues;

[0102] Step 7: When the assembly operation is performed to time t2, repeat steps 3-5 to calculate the second x-axis safety displacement, second y-axis safety displacement and second z-axis safety displacement of the actuator along the x, y and z axes of the sensor coordinate system at time t2, and the second x-axis safety angle, second y-axis safety angle and second z-axis safety angle of the actuator rotating around the x, y and z axes of the sensor coordinate system at time t2;

[0103] Read in real time the second x-axis displacement, the second y-axis displacement, and the second z-axis displacement of the actuator along the x, y, and z axes of the sensor coordinate system caused by the force applied to the actuator during the process from time t2 to time t3, as well as the second x-axis angle value, the second y-axis angle value, and the second z-axis angle value of the actuator rotating around the x, y, and z axes of the sensor coordinate system;

[0104] If the second x-axis displacement, the second y-axis displacement and the second z-axis displacement reach the corresponding second x-axis safety displacement, the second y-axis safety displacement and the second z-axis safety displacement, or the second x-axis angle, the second y-axis angle and the second z-axis angle reach the corresponding second x-axis safety angle, the second y-axis safety angle and the second z-axis safety angle, the actuator stops suddenly and cannot be moved or rotated;

[0105] If the second x-axis displacement, the second y-axis displacement, and the second z-axis displacement do not reach the corresponding second x-axis safety displacement, the second y-axis safety displacement, and the second z-axis safety displacement, or the second x-axis angle, the second y-axis angle, and the second z-axis angle do not reach the corresponding second x-axis safety angle, the second y-axis safety angle, and the second z-axis safety angle, the assembly operation continues;

[0106] And so on, until the actuator stops suddenly or the assembly work is completed.

[0107] In the process from the start to the completion of the workpiece assembly operation in this embodiment, the actuator emergency stop is not triggered.

[0108] The above description is only a preferred embodiment of the present invention, but the present invention is not limited thereto. All equivalent replacements or equivalent changes of the concepts and technical solutions according to the present invention should be included in the protection scope of the present invention.

Claims

1. A method for controlling an actuator of a hand-pushing robot, wherein the actuator and the six-dimensional force sensor are installed at the end of the robot, characterized in that The steps include: Step 1: Establish a base coordinate system with the base of the robot as the origin, and establish a sensor coordinate system with the six-dimensional force sensor as the origin; Step 2: Control the actuator to perform at least three posture changes in the loaded state with the workpiece picked up and the unloaded state without the workpiece picked up, and calculate the load center of gravity coordinates of the actuator in the sensor coordinate system according to the following formula (1): （1）； In formula (1), … is the first x-axis load gravity moment component of the actuator from the first pose to the nth pose on the x-axis of the sensor coordinate system; … is the first y-axis load gravity moment component of the actuator from the first pose to the nth pose on the y-axis of the sensor coordinate system; … is the first z-axis load gravity moment component of the actuator from the first pose to the nth pose on the z-axis of the sensor coordinate system; , , They are respectively the sensor coordinate system when the actuator is not subject to external force , , The x-axis no-load gravity moment component, the y-axis no-load gravity moment component and the z-axis no-load gravity moment component on the axis; … is the first x-axis load gravity component of the actuator from the first pose to the nth pose on the x-axis of the sensor coordinate system; … is the first y-axis load gravity component of the actuator from the first pose to the nth pose on the y-axis of the sensor coordinate system; … is the first z-axis load gravity component of the actuator from the first pose to the nth pose on the z-axis of the sensor coordinate system; Step 2.1: Calculate the actuator load gravity according to formula (2) （2）； In formula (2), … is the first transformation matrix between the sensor coordinate system and the base coordinate system from the first pose to the nth pose; , , They are respectively the x-axis no-load gravity component, the y-axis no-load gravity component and the z-axis no-load gravity component on the x, y and z-axes of the sensor coordinate system when the actuator is not subjected to external force; , , are respectively a second x-axis load gravity component, a second y-axis load gravity component, and a second z-axis load gravity component of the actuator on the x, y, and z axes of the base coordinate system; is the third-order identity matrix; Step 2.2: Calculate the load gravity according to formula (3) The third x-axis load gravity component on the x, y, z axes of the sensor coordinate system , the third y-axis load gravity component and the third z-axis load gravity component （3）； In formula (3), is the second transformation matrix between the sensor coordinate system and the base coordinate system; T is the transpose; Step 2.3: Calculate the load gravity according to formula (4) The second x-axis load gravity moment component on the x, y, z axes of the sensor coordinate system , the second y-axis load gravity moment component and the second z-axis load gravity moment component （4）； Step 3: Set the interval time Q for reading the six-dimensional force sensor readings; the time when the six-dimensional force sensor readings are read for the first time is taken as time t1, and the time when the six-dimensional force sensor readings are read for the second time after the interval time Q is taken as time t2, and so on until time tn; Push the actuator to perform workpiece assembly operations and perform the following steps; Step 3.1: Read the time t1 in the sensor coordinate system , , The x-axis total force component on the axis , total force component on the y-axis and the z-axis total force component , and in the sensor coordinate system , , The x-axis total moment component on the axis , total moment component of y-axis and the total moment component in the z-axis ; Step 3.2: Calculate the x-axis force component of the force applied to the actuator at time t1 on the x, y, and z axes of the sensor coordinate system according to equation (5): , y-axis applied force component and the z-axis force component , and the torque applied to the actuator at time t1 is the torque component applied to the x-axis on the x, y, and z axes of the sensor coordinate system , the torque component applied on the y-axis and the z-axis applied moment component （5）； Step 3.3: Calculate the x-axis acceleration of the actuator moving along the x, y, and z axes of the sensor coordinate system at time t1 using equation (6): , y-axis moving acceleration , z-axis moving acceleration , x-axis moving speed , y-axis moving speed and z-axis moving speed （6）； In formula (6), m is the total mass of the actuator and the workpiece; is the virtual damping, ; is the moving speed at the moment before t1. For the first reading of the six-dimensional force sensor at t1, the moving speed at the moment before t1 is , , All are 0; The x-axis angular acceleration of the actuator rotating around the x, y, and z axes of the sensor coordinate system at time t1 is calculated using formula (7): , y-axis angular acceleration 、z-axis angular acceleration , x-axis angular velocity , y-axis angular velocity and the z-axis angular velocity （7）； In formula (7), j is the moment of inertia of the actuator; is the angular velocity at the moment before t1. For the first reading of the six-dimensional force sensor at t1, the angular velocity at the moment before t1 is , , All are 0; Step 4: Determine the x-axis distance range of the actuator moving along the x, y, and z axes of the sensor coordinate system according to the on-site working conditions , y-axis distance range And z-axis distance range , and the x-axis angle range of the actuator rotating around the x, y, and z axes of the sensor coordinate system Y-axis angle range and z-axis angle range ; The x-axis movement coefficient of the actuator moving along the x, y, and z axes of the sensor coordinate system is calculated according to the following formula (8): , y-axis movement coefficient and z-axis movement coefficient （8）； In formula (8), is the first safety estimation threshold for the actuator to move along the x, y, and z axes of the sensor coordinate system, respectively, and is determined based on experience. ; , and are the x-axis initial movement coefficient, the y-axis initial movement coefficient and the z-axis initial movement coefficient on the x, y and z-axes of the sensor coordinate system respectively; The x-axis rotation coefficient of the actuator rotating around the x, y, and z axes of the sensor coordinate system is calculated by the following formula (9): , y-axis rotation coefficient and the z-axis rotation coefficient （9）； In formula (9), , and are the x-axis angle, y-axis angle and z-axis angle of the actuator rotating around the x, y and z axes of the sensor coordinate system; is the second safety estimation threshold for the actuator to rotate around the x, y, and z axes of the sensor coordinate system, respectively, and is determined based on experience. ; , and are respectively the x-axis initial rotation coefficient, the y-axis initial rotation coefficient and the z-axis initial rotation coefficient on the x, y and z-axes of the sensor coordinate system; Step 5: Calculate the first x-axis safe displacement of the actuator along the x, y, and z axes of the sensor coordinate system at time t1 according to formula (10): , the first y-axis safety displacement and the first z-axis safety displacement , and the first x-axis safety angle of the actuator rotating around the x, y, and z axes of the sensor coordinate system at time t1 , the first y-axis safety angle and the first z-axis safety angle （10）； Step 6: reading in real time a first x-axis displacement, a first y-axis displacement, and a first z-axis displacement of the actuator along the x, y, and z axes of the sensor coordinate system caused by the force applied to the actuator during the process from time t1 to time t2, as well as a first x-axis angle value, a first y-axis angle value, and a first z-axis angle value of the actuator rotating around the x, y, and z axes of the sensor coordinate system; If the first x-axis displacement, the first y-axis displacement and the first z-axis displacement reach the corresponding first x-axis safety displacement, the first y-axis safety displacement and the first z-axis safety displacement, or the first x-axis angle, the first y-axis angle and the first z-axis angle reach the corresponding first x-axis safety angle, the first y-axis safety angle and the first z-axis safety angle, the actuator stops suddenly and cannot be moved or rotated; If the first x-axis displacement, the first y-axis displacement, and the first z-axis displacement do not reach the corresponding first x-axis safety displacement, the first y-axis safety displacement, and the first z-axis safety displacement, or the first x-axis angle, the first y-axis angle, and the first z-axis angle do not reach the corresponding first x-axis safety angle, the first y-axis safety angle, and the first z-axis safety angle, the assembly operation continues; Step 7: When the assembly operation is performed to time t2, repeat steps 3-5 to calculate the second x-axis safety displacement, the second y-axis safety displacement and the second z-axis safety displacement of the actuator moving along the x, y and z axes of the sensor coordinate system at time t2, and the second x-axis safety angle, the second y-axis safety angle and the second z-axis safety angle of the actuator rotating around the x, y and z axes of the sensor coordinate system at time t2; Read in real time the second x-axis displacement, the second y-axis displacement, and the second z-axis displacement of the actuator along the x, y, and z axes of the sensor coordinate system caused by the force applied to the actuator during the process from time t2 to time t3, as well as the second x-axis angle value, the second y-axis angle value, and the second z-axis angle value of the actuator rotating around the x, y, and z axes of the sensor coordinate system; If the second x-axis displacement, the second y-axis displacement and the second z-axis displacement respectively reach the corresponding second x-axis safety displacement, the second y-axis safety displacement and the second z-axis safety displacement, or the second x-axis angle value, the second y-axis angle value and the second z-axis angle value respectively reach the corresponding second x-axis safety angle value, the second y-axis safety angle value and the second z-axis safety angle value, the actuator stops suddenly and cannot be moved or rotated; If the second x-axis displacement, the second y-axis displacement, and the second z-axis displacement do not reach the corresponding second x-axis safety displacement, the second y-axis safety displacement, and the second z-axis safety displacement, or the second x-axis angle, the second y-axis angle, and the second z-axis angle do not reach the corresponding second x-axis safety angle, the second y-axis safety angle, and the second z-axis safety angle, the assembly operation continues; And so on, until the actuator is emergency stopped or the assembly operation is completed.

2. The actuator control method of a hand-push robot according to claim 1, characterized in that: In step 4, the x, y, z axis distance ranges and x, y, z axis angle ranges determined according to the on-site working conditions are determined using a visual calibration method.

3. The actuator control method of a hand-push robot according to claim 1, characterized in that: The virtual damping in step 3.3 Adjustable, By increasing the value, the acceleration and speed of the actuator moving along the sensor coordinate system x, y, and z under the same applied force are controlled to be faster.

Images