A robotic arm position control assembly method based on Gaussian function stiffness transformation

Abstract

The invention discloses a method for position control and assembly of a robot arm based on Gaussian function stiffness transformation, and belongs to the field of robot arm control. The invention introduces a stiffness matrix that presents Gaussian transformation over time, so that the robot arm can run at a safer speed in a free space while ensuring high precision. In the contact space, the lower limit of the stiffness is set to a certain threshold value, so that the robot arm can maintain a certain flexibility during the assembly process, and at the same time, it can avoid the problem that the stiffness matrix becomes too small due to the use of a Gaussian function that increases over time, resulting in the problem that it is too flexible to counteract the environmental stiffness and cannot complete the assembly.

Description

Technical Field

[0001] The invention relates to the field of mechanical arm control, and in particular to a mechanical arm position control assembly method based on Gaussian function stiffness transformation. Background Art

[0002] The position control of the robot arm is mainly divided into joint space control and Cartesian space control. The control of the robot arm joint space requires the inverse kinematics module to map the Cartesian task trajectory to the joint space, and then track the trajectory through the joint layer controller after obtaining the joint space trajectory. For the joint layer controller, the simplest is when the robot arm moves slowly and the transmission part uses a large reduction ratio. At this time, the robot arm system can be regarded as a linear system, and its nonlinear coupling part can be ignored. At this time, PD control is used for each joint. Although the robot arm joint space controller based on PD control of each independent joint has asymptotic stability, it cannot guarantee the stability of all task points.

[0003] The above control does not take into account the dynamic model of the robot arm, so it is called "non-model-based control". When the robot arm moves at a fast speed, the nonlinear coupling between the various parts of the robot arm is serious. If precise position control is to be performed, the dynamic model of the robot arm needs to be considered. That is, "model-based control", also known as "dynamic control". This model-based robot arm control scheme can make the robot arm have better dynamic performance. The control of the model-based robot arm usually includes an inner control loop and an outer control loop, in which the inner control loop mainly feeds back the dynamic compensation, which decouples the nonlinear control system into a linear system. The controller in the outer control loop can adopt a PD controller, an adaptive controller, etc.

[0004] Both model-based PD control and non-model-based PD control have the same problem. When the stiffness matrix used as the gain is large, although it can ensure that the robot arm can reach the specified point more accurately, the movement speed of the robot arm will increase with the increase of the gain, and safety problems are very likely to occur. Summary of the invention

[0005] In view of the problems existing in the above-mentioned technology, the purpose of the present invention is to provide a robot arm position control assembly method based on Gaussian function stiffness transformation. This method introduces a stiffness matrix that presents Gaussian transformation over time, so that the robot arm can run at a safer speed in free space while ensuring high precision; in the contact space, the lower limit of the stiffness is set to a certain threshold (depending on the robot arm and the environment, the stiffness of 40N / m is used as the lower limit of the stiffness in the present invention), so that the robot arm can maintain a certain flexibility during the assembly process, and at the same time avoid the problem that the stiffness matrix becomes too small due to the use of Gaussian function Over time, resulting in too much flexibility to counteract the environmental stiffness and unable to complete the assembly.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A method for position control assembly of a robotic arm based on Gaussian function stiffness transformation comprises the following steps:

[0008] 1) Camera calibration: Use a depth camera and the eye-in-hand calibration method to calibrate the position of the camera relative to the end effector. The homogeneous transformation matrix of the end effector coordinate system relative to the robot base coordinate system is obtained by the robot itself. The relationship between the end effector coordinate system and the robot base coordinate system is shown in formula (1-1):

[0009] P b =A b2e P e (1-1)

[0010]

[0011] The homogeneous transformation matrix of the end effector coordinate system relative to the base coordinate system of the robot arm at the initial position of the robot arm is obtained by formula (1-1), which is denoted as A b2e_start , the relationship between the end effector coordinate system and the robot base coordinate system at the initial position is shown in formula (1-3):

[0012] P b =A b2e_start P e (1-3)

[0013]

[0014] The rotation and translation matrix of the world coordinate system relative to the camera coordinate system is obtained by the Aruco code calibration method. The relationship between the world coordinate system and the camera coordinate system is shown in formula (1-5):

[0015] P c =A c2w P w (1-5)

[0016]

[0017] The rotation and translation matrix of the world coordinate system relative to the robot base coordinate system is obtained by manual measurement. The relationship between the world coordinate system and the robot base coordinate system is shown in formula (1-7):

[0018] A b2w P w =P b (1-7)

[0019]

[0020] The relationship between the camera coordinate system and the end effector coordinate system is shown in formula (1-9):

[0021] P e =A e2c P c (1-9)

[0022]

[0023] The combined equations (1-3), (1-5), (1-7), and (1-9) give the homogeneous transformation matrix of the camera coordinate system relative to the end effector coordinate system, as shown in equation (1-11):

[0024]

[0025] Among them, P e , P c , P w , P b Respectively represent the coordinate system vector of the end effector, the camera coordinate system vector, the world coordinate system vector and the robot base coordinate system vector; A b2e , A b2e_start , A e2c , A c2w , A b2w They represent the homogeneous transformation matrix of the end effector coordinate system relative to the robot base coordinate system, the homogeneous transformation matrix of the initial position end effector coordinate system relative to the robot base coordinate system, the homogeneous transformation matrix of the camera coordinate system relative to the end effector coordinate system, the homogeneous transformation matrix of the world coordinate system relative to the camera coordinate system, and the homogeneous transformation matrix of the world coordinate system relative to the robot base coordinate system respectively; R b2e , T b2e They represent the rotation matrix and translation matrix of the end effector coordinate system relative to the robot base coordinate system; R b2e_start , T b2e_start They represent the rotation matrix and translation matrix of the end effector coordinate system relative to the robot base coordinate system at the initial position; R e2c , T e2c They represent the rotation matrix and translation matrix of the camera coordinate system relative to the end effector coordinate system; R c2w , T c2w Respectively represent the rotation matrix and translation matrix of the world coordinate system relative to the camera coordinate system; R b2w , T b2w They represent the rotation matrix and translation matrix of the world coordinate system relative to the robot base coordinate system respectively;

[0026] The relationship between the pixel coordinate system and the camera coordinate system is shown in formula (1-12):

[0027]

[0028] The camera internal parameters obtained from the official example code of the camera are shown in formula (1-13):

[0029]

[0030] Then convert the pixel coordinate system to the camera coordinate system, and combine equations (1-12) and (1-13) to get equation (1-14):

[0031]

[0032] Among them, u and v represent pixel coordinates respectively; x c ,y c 、z c They represent the camera coordinates respectively; K represents the camera internal parameters; f x 、f y Respectively represent the focal length of the camera in the x and y directions; c x 、c y Respectively represent the offset of the camera optical axis in the x and y directions in the image coordinate system; by combining (1-1), (1-9), (1-11), and (1-14), we can get the relationship between the pixel coordinate system and the robot base coordinate system, as shown in formula (1-15):

[0033]

[0034] 2) Target detection and positioning: Use YOLOv5 target detection to distinguish the assembly object and the base, and obtain the center point coordinates and depth information of the assembly object and the base, which are recorded as (x1, y1, z1) and (x2, y2, z2) respectively;

[0035] 3) Posture estimation: All possible holding postures of the visible local point cloud are calculated through the 6Dof-Graspnet algorithm, and the optimal holding posture is obtained through evaluation network reasoning to obtain the postures of the assembled object and the base, which are recorded as R1 and R2 respectively;

[0036] 4) Serial communication: The host computer responsible for visual processing sends the two position information (x1, y1, z1) and (x2, y2, z2) obtained in step 2) and the two posture information R1 and R2 obtained in step 3) to the host computer responsible for robot arm control through serial communication;

[0037] 5) Robotic arm position control: According to the position information (x1, y1, z1) of the assembly object obtained by transmission in step 4), the robot arm position control method based on Gaussian function stiffness transformation proposed in this paper is used to send instructions to the robot arm through the robot arm control cabinet to make it reach the point (x1, y1, z1);

[0038] 6) Grasping: After reaching the point (x1, y1, z1), when contact is made with the assembly object, the lower limit of stiffness is set, the position information R1 of the assembly object obtained by transmitting in step 4) is adjusted, and the posture of the end effector of the robot arm is closed to grasp the object;

[0039] 7) Robotic arm position control: After the grasping is successful, according to the position information (x2, y2, z2) of the base obtained by the transmission in step 4), the robot arm position control method based on Gaussian function stiffness transformation proposed in this paper is also used to send instructions to the robot arm through the robot arm control cabinet to make it reach the point (x2, y2, z2);

[0040] 8) Assembly: After reaching the point (x2, y2, z2), the base posture information R2 obtained by the transmission in step 4) is adjusted to complete the assembly action by adjusting the posture of the end effector of the robot arm;

[0041] 9) Reset: After assembly is completed, release the end effector and command the robot arm to return to the initial position, ending the entire assembly process.

[0042] Furthermore, in the step 4), the robot arm control cabinet opens at the beginning to wait for the posture information sent by the upper computer for visual processing, and will proceed to the next step only after it is received, otherwise it will keep waiting.

[0043] Furthermore, in step 5), the robot arm PD control equation is as shown in formula (1-1):

[0044]

[0045] The Gaussian function proposed by the present invention is shown as (1-2):

[0046]

[0047] Where τ represents the torque of the robot arm; J represents the Jacobian matrix; K p Represents the proportional gain of PD control and represents the stiffness matrix of the robot in robot control; K d represents the differential control of PD control, and represents the damping matrix of the robot in robot control; x d represents the expected position; x represents the actual position; Indicates the expected speed; Indicates actual speed; K g It represents the gain of the Gaussian function, which is the magnification factor of the y-axis; slide is the scaling factor of the x-axis; t represents time; μ is the translation amount of the x-axis; σ represents the standard deviation.

[0048] The beneficial effects of the present invention are:

[0049] 1) A Gaussian function stiffness transformation method is proposed as the gain of PD control to control the position of the robot arm. In free space, the robot arm can approach the target point at a safer speed while maintaining high precision.

[0050] 2) The lower limit of stiffness set in the contact space can ensure that the accuracy and safety of position control in the free space are not affected and the flexibility of assembly in the contact space is maintained. At the same time, it can also avoid the problem that the stiffness matrix becomes too small due to the use of Gaussian function over time, resulting in too flexibility to counteract the environmental stiffness and the inability to complete the assembly. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a flow chart of a method implementation of an embodiment of the present invention;

[0052] Figure 2 It is a Gaussian function curve diagram proposed in an embodiment of the present invention. DETAILED DESCRIPTION

[0053] The present invention will be further described in detail below in conjunction with specific embodiments.

[0054] The purpose of the present invention is to propose a method for position control assembly of a robot arm based on Gaussian function stiffness transformation. The method introduces a stiffness matrix that presents Gaussian transformation over time, so that the robot arm can run at a safer speed in free space while ensuring high precision; in the contact space, the lower limit of the stiffness is set to a certain threshold (depending on the robot arm and the environment, and the stiffness of 40N / m is used as the lower limit of the stiffness in the present invention), so that the robot arm can maintain a certain flexibility during the assembly process, and at the same time avoid the problem that the stiffness matrix becomes too small due to the use of a Gaussian function that increases over time, resulting in the problem that it is too flexible to counteract the environmental stiffness and cannot complete the assembly.

[0055] In this embodiment, self-made assembly parts are selected as assembly objects, and a YOLOv5 data set of assembly parts is prepared, and the 6Dof-Graspnet posture detection parameters trained by NVIDIA are used as the weights of posture detection in the present invention. Two host computers are used for visual processing and robotic arm control respectively.

[0056] A method for position control assembly of a robotic arm based on Gaussian function stiffness transformation, the specific steps are as follows:

[0057] Step 1: Camera calibration: The present invention uses a depth camera and adopts the eye-in-hand hand-eye calibration method to calibrate the position of the camera relative to the end effector. The rotation and translation matrix of the end effector coordinate system relative to the robot base coordinate system can be obtained by the robot itself. The relationship between the end effector coordinate system and the robot base coordinate system is shown in formula (1-3):

[0058] P b =A b2e P e (1-3)

[0059]

[0060] The homogeneous transformation matrix of the end effector coordinate system relative to the base coordinate system of the robot arm at the initial position of the robot arm is obtained by formula (1-3), which is denoted as A b2e_start , the relationship between the end effector coordinate system and the robot base coordinate system at the initial position is shown in formula (1-5):

[0061] P b =A b2e_start P e (1-5)

[0062]

[0063] The rotation and translation matrix of the world coordinate system relative to the camera coordinate system is obtained by the Aruco code calibration method. The relationship between the world coordinate system and the camera coordinate system is shown in formula (1-7):

[0064] P c =A c2w P w (1-7)

[0065]

[0066] The rotation and translation matrix of the world coordinate system relative to the robot base coordinate system can be obtained by manual measurement. The relationship between the world coordinate system and the robot base coordinate system is shown in formula (1-9):

[0067] A b2w P w =P b (1-9)

[0068]

[0069] The relationship between the camera coordinate system and the end effector coordinate system is shown in formula (1-11):

[0070] P e =A e2c P c (1-11)

[0071]

[0072] The combined equations (1-5), (1-7), (1-9), and (1-11) give the homogeneous transformation matrix of the camera coordinate system relative to the end effector coordinate system, as shown in equation (1-13):

[0073]

[0074] Among them, P e , P c , P w , P b Respectively represent the coordinate system vector of the end effector, the camera coordinate system vector, the world coordinate system vector and the robot base coordinate system vector; A b2e , A b2e_start , A e2c , A c2w , A b2w They represent the homogeneous transformation matrix of the end effector coordinate system relative to the robot base coordinate system, the homogeneous transformation matrix of the initial position end effector coordinate system relative to the robot base coordinate system, the homogeneous transformation matrix of the camera coordinate system relative to the end effector coordinate system, the homogeneous transformation matrix of the world coordinate system relative to the camera coordinate system, and the homogeneous transformation matrix of the world coordinate system relative to the robot base coordinate system respectively; R b2e , T b2e They represent the rotation matrix and translation matrix of the end effector coordinate system relative to the robot base coordinate system; R b2e_start , T b2e_start They represent the rotation matrix and translation matrix of the end effector coordinate system relative to the robot base coordinate system at the initial position; R e2c , T e2c They represent the rotation matrix and translation matrix of the camera coordinate system relative to the end effector coordinate system; R c2w , T c2w Respectively represent the rotation matrix and translation matrix of the world coordinate system relative to the camera coordinate system; R b2w , T b2w They represent the rotation matrix and translation matrix of the world coordinate system relative to the robot base coordinate system respectively; the relationship between the pixel coordinate system and the camera coordinate system is shown in formula (1-14):

[0075]

[0076] The camera internal parameters obtained from the official example code of the camera are shown in formula (1-15):

[0077]

[0078] Then convert the pixel coordinate system to the camera coordinate system, and combine equations (1-14) and (1-15) to get equation (1-16):

[0079]

[0080] Among them, u and v represent pixel coordinates respectively; x c ,yc 、z c They represent the camera coordinates respectively; K represents the camera internal parameters; f x 、f y Respectively represent the focal length of the camera in the x and y directions; c x 、c y Respectively represent the offset of the camera optical axis in the x and y directions in the image coordinate system; by combining (1-3), (1-11), (1-13), and (1-16), we can get the relationship between the pixel coordinate system and the robot base coordinate system, as shown in formula (1-17):

[0081]

[0082] Step 2: Target detection and positioning: Use YOLOv5 target detection to distinguish the assembly object and the base, and obtain the center point coordinates and depth information of the assembly object and the base, which are recorded as (x1, y1, z1) and (x2, y2, z2) respectively.

[0083] Step 3: Posture estimation: All possible holding postures of the visible local point cloud are calculated through the 6Dof-Graspnet algorithm, and the optimal holding posture is obtained through evaluation network reasoning to obtain the postures of the assembled object and the base, which are denoted as R1 and R2 respectively.

[0084] Step 4: Serial port communication: The host computer responsible for visual processing sends the two position information (x1, y1, z1) and (x2, y2, z2) obtained in step 2 and the two posture information R1 and R2 obtained in step 3 to the host computer responsible for robot arm control through serial port communication.

[0085] Step 5: Robot position control: According to the position information (x1, y1, z1) of the assembly object obtained by the transmission in step 4, the robot position control method based on Gaussian function stiffness transformation proposed in this paper is used to send instructions to the robot through the robot control cabinet to make it reach the point (x1, y1, z1). Equation (1-18) is the robot PD control equation:

[0086]

[0087] Formula (1-19) is the given Gaussian function:

[0088]

[0089] Where τ represents the torque of the robot arm; J represents the Jacobian matrix; K p Represents the proportional gain of PD control and represents the stiffness matrix of the robot in robot control; K d represents the differential control of PD control, and represents the damping matrix of the robot in robot control; x drepresents the expected position; x represents the actual position; Indicates the expected speed; Indicates actual speed; K g It represents the gain of the Gaussian function, which is the magnification factor of the y-axis; slide is the scaling factor of the x-axis; t represents time; μ is the translation amount of the x-axis; σ represents the standard deviation.

[0090] Step 6: Grasping: After reaching the point (x1, y1, z1), when contact is made with the assembly object, the lower limit of stiffness is set to 40N / m. The assembly object posture information R1 obtained by the transmission in step 4 is used to adjust the posture of the end effector of the robot arm, and the gripper is closed to grasp the object.

[0091] Step 7: Robotic arm position control: After the grasping is successful, according to the position information (x2, y2, z2) of the base obtained by transmitting in step 4, the robotic arm position control method based on Gaussian function stiffness transformation proposed in this paper is also used to send instructions to the robotic arm through the robotic arm control cabinet to make it reach the point (x2, y2, z2).

[0092] Step 8: Assembly: After reaching the point (x2, y2, z2), the base posture information R2 obtained by transmitting in step 4 is adjusted to adjust the posture of the end effector of the robot arm to complete the assembly action.

[0093] Step 9: Reset: After assembly is completed, release the end effector and command the robot arm to return to the initial position, ending the entire assembly process.

[0094] The above are preferred embodiments of the present invention. Any changes made according to the technical solution of the present invention, as long as the resulting functions do not exceed the scope of the technical solution of the present invention, belong to the protection scope of the present invention.

Claims

1. A method for position control assembly of a robotic arm based on Gaussian function stiffness transformation, characterized in that: The following steps are involved: 1) Camera calibration: Use a depth camera and the eye-in-hand calibration method to calibrate the position of the camera relative to the end effector. The homogeneous transformation matrix of the end effector coordinate system relative to the robot base coordinate system is obtained by the robot itself. The relationship between the end effector coordinate system and the robot base coordinate system is shown in formula (1-1): P b =A b2e P e (1-1) The homogeneous transformation matrix of the end effector coordinate system relative to the base coordinate system of the robot arm at the initial position of the robot arm is obtained by formula (1-1), which is denoted as A b2e_start , the relationship between the end effector coordinate system and the robot base coordinate system at the initial position is shown in formula (1-3): P b =A b2e_start P e (1-3) The rotation and translation matrix of the world coordinate system relative to the camera coordinate system is obtained by the Aruco code calibration method. The relationship between the world coordinate system and the camera coordinate system is shown in formula (1-5): P c =A c2w P w (1-5) The rotation and translation matrix of the world coordinate system relative to the robot base coordinate system is obtained by manual measurement. The relationship between the world coordinate system and the robot base coordinate system is shown in formula (1-7): A b2w P w =P b (1-7) The relationship between the camera coordinate system and the end effector coordinate system is shown in formula (1-9): P e =A e2c P c (1-9) The combined equations (1-3), (1-5), (1-7), and (1-9) give the homogeneous transformation matrix of the camera coordinate system relative to the end effector coordinate system, as shown in equation (1-11): Among them, P e , P c , P w , P b Respectively represent the coordinate system vector of the end effector, the camera coordinate system vector, the world coordinate system vector and the robot base coordinate system vector; A b2e , A b2e_start , A e2c , A c2w , A b2w They represent the homogeneous transformation matrix of the end effector coordinate system relative to the robot base coordinate system, the homogeneous transformation matrix of the initial position end effector coordinate system relative to the robot base coordinate system, the homogeneous transformation matrix of the camera coordinate system relative to the end effector coordinate system, the homogeneous transformation matrix of the world coordinate system relative to the camera coordinate system, and the homogeneous transformation matrix of the world coordinate system relative to the robot base coordinate system respectively; R b2e , T b2e They represent the rotation matrix and translation matrix of the end effector coordinate system relative to the robot base coordinate system; R b2e_start , T b2e_start They represent the rotation matrix and translation matrix of the end effector coordinate system relative to the robot base coordinate system at the initial position; R e2c , T e2c They represent the rotation matrix and translation matrix of the camera coordinate system relative to the end effector coordinate system; R c2w , T c2w Respectively represent the rotation matrix and translation matrix of the world coordinate system relative to the camera coordinate system; R b2w , T b2w They represent the rotation matrix and translation matrix of the world coordinate system relative to the robot base coordinate system respectively; The relationship between the pixel coordinate system and the camera coordinate system is shown in formula (1-12): The camera internal parameters obtained from the official example code of the camera are shown in formula (1-13): Then convert the pixel coordinate system to the camera coordinate system, and combine equations (1-12) and (1-13) to get equation (1-14): Among them, u and v represent pixel coordinates respectively; x c ,y c 、z c They represent the camera coordinates respectively; K represents the camera internal parameter; f x 、f y Respectively represent the focal length of the camera in the x and y directions; c x 、c y Respectively represent the offset of the camera optical axis in the x and y directions in the image coordinate system; by combining (1-1), (1-9), (1-11), and (1-14), we can get the relationship between the pixel coordinate system and the robot base coordinate system, as shown in formula (1-15): 2) Target detection and positioning: Use YOLOv5 target detection to distinguish the assembly object and the base, and obtain the center point coordinates and depth information of the assembly object and the base, which are recorded as (x1, y1, z1) and (x2, y2, z2) respectively; 3) Posture estimation: All holding postures of the visible local point cloud are calculated through the 6Dof-Graspnet algorithm, and the best holding posture is obtained through evaluation network reasoning to obtain the postures of the assembled object and the base, which are recorded as R1 and R2 respectively; 4) Serial communication: The host computer responsible for visual processing sends the two position information (x1, y1, z1) and (x2, y2, z2) obtained in step 2) and the two posture information R1 and R2 obtained in step 3) to the host computer responsible for robot arm control through serial communication; 5) Robot arm position control: According to the position information (x1, y1, z1) of the assembly object obtained by the transmission in step 4), the robot arm control cabinet sends a command to the robot arm to make it reach the point (x1, y1, z1); 6) Grasping: After reaching the point (x1, y1, z1), when contact is made with the assembly object, the lower limit of stiffness is set; the posture information R1 of the assembly object obtained by transmitting in step 4) is adjusted, and the posture of the end effector of the robot arm is closed to grasp the object; 7) Robot arm position control: After the grasping is successful, according to the position information (x2, y2, z2) of the base obtained by the transmission in step 4), the robot arm control cabinet sends a command to the robot arm to make it reach the point (x2, y2, z2); 8) Assembly: After reaching the point (x2, y2, z2), the base posture information R2 obtained by the transmission in step 4) is adjusted to complete the assembly action by adjusting the posture of the end effector of the robot arm; 9) Reset: After assembly is completed, release the end effector and command the robot arm to return to the initial position, ending the entire assembly process.

2. A method for manipulator position control assembly based on Gaussian function stiffness transformation according to claim 1, characterized in that: In the step 4), the robot arm control cabinet opens at the beginning to wait for the posture information sent by the upper computer for visual processing, and will proceed to the next step only after it is received, otherwise it will keep waiting.

3. The method for manipulator position control assembly based on Gaussian function stiffness transformation according to claim 1, characterized in that: In step 5), the robot PD control equation is shown in formula (1-16): The Gaussian function is shown in (1-17): Where τ represents the torque of the robot arm; J represents the Jacobian matrix; K p Represents the proportional gain of PD control and represents the stiffness matrix of the robot in robot control; K d represents the differential control of PD control, and represents the damping matrix of the robot in robot control; x d represents the expected position; x represents the actual position; Indicates the expected speed; Indicates actual speed; K g It represents the gain of the Gaussian function, which is the magnification factor of the y-axis; slide is the scaling factor of the x-axis; t represents time; μ is the translation amount of the x-axis; σ represents the standard deviation.

Images