Method and system for cooperatively controlling movement of mechanical arm based on visual servo and force sense admittance

Abstract

The invention belongs to the technical field related to robot visual control, and discloses a method and system for cooperatively controlling a mechanical arm to move based on visual servo and force sense admittance. The method comprises the steps that a mechanical arm moves to an assembly target position, and an image collection device collects an image of a target point at the assembly target position as an expected image; the tail end of the mechanical arm clamps the to-be-assembled workpiece to move to an assembly target position, an image collection device collects an image of a target point in real time in the moving process of the tail end of the mechanical arm, and the image collected in real time is compared with an expected image so as to calculate the movement speed vc of the tail end of the mechanical arm; when the to-be-assembled workpiece is in contact with the pin at the assembly target position, the force sensor measures the force and the torque during contact, and the movement speed vf of the tail end of the mechanical arm is calculated through the force and the torque; and calculating the movement speed of the tail end of the mechanical arm at the next moment by utilizing the movement speeds vc and vf of the mechanical arm. According to the invention, the problem that visual control and force control cannot be cooperatively controlled in the prior art is solved.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot vision control, and more specifically, relates to a method and system for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance. Background Art

[0002] Assembly is a common form of industrial robot production. Factories are usually equipped with sensors to guide robot assembly. Visual servoing with the help of cameras and impedance-admittance control with the help of force sensors are commonly used guidance methods.

[0003] Visual servoing has good flexibility and robustness, but the assembly accuracy is overly dependent on the accuracy of the camera itself and the accuracy of the calibrated camera parameters. Especially for industrial scenarios, it is difficult to achieve high-precision servo control. Force sensors can achieve higher accuracy and safety in guiding the assembly process, but usually require an out-of-hole search phase for shaft-hole docking. The search strategy is usually a spiral search method. This stage not only requires a full analysis of the contact method between the shaft and the hole, but also may cause damage to the robot during the search. Multi-sensor fusion can achieve multi-level information fusion and reduce the accuracy requirements of a single sensor, but the combination of vision and force sensors is not simple.

[0004] Therefore, there is an urgent need for a method that can achieve collaborative control combining vision and force sensors. Summary of the invention

[0005] In view of the above defects or improvement needs of the prior art, the present invention provides a method and system for collaboratively controlling the movement of a robotic arm based on visual servoing and force sensing admittance, which solves the problem in the prior art that visual control and force control cannot be collaboratively controlled.

[0006] To achieve the above object, according to one aspect of the present invention, a method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance is provided, the method comprising the following steps:

[0007] The robotic arm connected with an image acquisition device and a force sensor at the end is moved to the assembly target position, and the image acquisition device acquires the image of the target point at the assembly target position as the expected image; the end of the robotic arm clamps the workpiece to be assembled and moves to the assembly target position with the workpiece to be assembled. During the movement of the end of the robotic arm, the image acquisition device acquires the image of the target point in real time, and compares the real-time acquired image with the expected image to calculate the movement speed v of the end of the robotic arm under the control of the image acquisition device c ;

[0008] When the workpiece to be assembled contacts the pin at the assembly target position, the force sensor measures the force and torque at the contact, and uses the force and torque to calculate the movement speed v of the end of the robot arm under the control of the force sensor. f;

[0009] Using the robot arm movement speed v c and v f Calculate the actual movement speed v of the end of the robotic arm at the next moment.

[0010] Further preferably, the movement speed v of the end of the robotic arm is c The calculation formula is as follows:

[0011]

[0012] Where η is the controller gain, YesL s The generalized inverse matrix, L s is the image Jacobian matrix of image feature s, s is the image feature of the real-time acquired image, s * is the image feature in the desired image.

[0013] Further preferably, the image Jacobian matrix L of the image feature s s The formula is as follows:

[0014]

[0015]

[0016] in, Represents the image features x g ,y g ,z g The corresponding image Jacobian matrix, L r1 ,L r2 ,L r3 Represent image features r1, r2, r z The corresponding image Jacobian matrix, p1, p2, p3, p4 represent the coordinates of the four target points in the image, L1, L2, L3, L4 represent the image Jacobian matrix of the four target points, I2 represents the second-order unit matrix, α represents the coordinates of the four target points along n 14 Directions to The unit vector of the direction.

[0017] Further preferably, the 14 Directions to The formula for the unit vector α of the direction is as follows:

[0018]

[0019] Among them, n 14 represents the unit vector along the direction from target point 1 to target point 4, is the expected image n 14The value of , ω represents the direction coefficient, and I2 represents the second-order unit matrix.

[0020] Further preferably, the force sensor controls the movement speed v of the end of the lower robotic arm f Follow the steps below to calculate:

[0021]

[0022] Among them, M is the inertia matrix of the robot arm, D is the damping matrix, K is the stiffness matrix, and F ext is the actual external contact force and torque, v is the actual movement speed of the end of the robot, X is the position of the end of the robot, is the acceleration of the end of the manipulator under force-sensing admittance control, v f0 It represents the speed of the robot end at the last moment, and Δt represents the time interval.

[0023] Further preferably, the real external contact force and torque are the differences between the force and torque of the force sensor when contact is made at the tool end and the force and torque formed by the gravity of the workpiece to be assembled.

[0024] Further preferably, the calculation formula of the movement speed of the end of the robotic arm at the next moment is as follows:

[0025] v=(1-λ)v c +λv f

[0026] Among them, v is the movement speed of the end of the robot arm at the next moment, and λ is the proportion weight.

[0027] Further preferably, the calculation formula of the proportion weight λ is as follows:

[0028]

[0029] Among them, ΔZ represents the height to which the end of the robot arm descends after the workpiece and the pin come into contact, and ΔZ * Indicates the total height to which the end-of-arm is expected to descend after the workpiece and pin make contact.

[0030] According to another aspect of the present invention, a system for collaboratively controlling the motion of a robotic arm based on visual servoing and force admittance is provided. The system includes an actuator for executing the above-mentioned method for collaboratively controlling the motion of a robotic arm based on visual servoing and force admittance.

[0031] According to another aspect of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the method for collaboratively controlling the motion of a robotic arm based on visual servoing and force admittance is implemented.

[0032] In general, the above technical solutions conceived by the present invention have the following beneficial effects compared with the prior art:

[0033] 1. The present invention calculates the movement speed of the robot arm from two angles of image and contact force through an image acquisition device and a force sensor, and then obtains the final movement speed of the robot arm by weighted summation of the movement speeds calculated from the two angles. This control method realizes the coordinated control of visual servoing and force control, and improves the flexibility and adaptability of the assembly system.

[0034] 2. The present invention utilizes common, visually identifiable target points as image feature points for visual acquisition. The combination of four target points can be processed and calculated to control the six degrees of freedom of the end of the robotic arm. The control method is simple and efficient, the image is easy to identify, and the visual system has high robustness.

[0035] 3. The movement process of the robot arm in the present invention includes two stages. In the first stage, the end of the robot arm moves toward the assembly target position, and only the image acquisition device acquires images. In the second stage, the workpiece at the end of the robot arm contacts the pin, and the force sensor starts working. In the two processes, the image acquisition device and the force sensor work together to calculate a speed change respectively, and the final speed of the end of the robot arm is calculated by weighted summation. As the robot arm gradually moves toward the target position, its height becomes lower and lower, the proportion of the image acquisition device becomes lower and lower, and the proportion of the force sensor becomes higher and higher. The smooth transition between the two ensures the assembly accuracy of the workpiece. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 is a flow chart of a method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance constructed according to a preferred embodiment of the present invention;

[0037] Figure 2 is a schematic diagram of a target point constructed according to a preferred embodiment of the present invention;

[0038] Figure 3 It is a schematic diagram of the structure of a system for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance constructed according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0040] like Figure 1As shown, an assembly method based on visual servoing and force sensing admittance collaborative control includes the following steps:

[0041] S1 Figure 3 As shown, the robot arm moves to the assembly target position, and the camera captures an image of the target point at the assembly position as the desired image;

[0042] The camera collects the image features of the target point in real time, and calculates the movement speed of the end of the robot arm from the current moment to the next moment in real time based on the difference between the expected image features and the current image features, thus completing the visual servo control;

[0043] like Figure 2 As shown, four target points are posted within the camera field of view at the robotic arm assembly position to form an irregular quadrilateral.

[0044] After the camera takes the picture, the image is binarized, the mask is used to extract the corresponding area of ​​the original image, and the threshold processing is used to extract and find the white area to obtain the image coordinates of the four target points, P1 (x1, y1), P2 (x2, y2), P3 (x3, y3), P4 (x4, y4), and define the image features

[0045] s=[x g y g z g r1 r2 r z ] T ∈R 6*1

[0046] s is calculated as follows:

[0047]

[0048] Among them, x g ,y g represents the coordinates of the center of mass, z g represents the area of ​​the closed region formed by the target point. In this embodiment, the closed region is a quadrilateral. g y g z g}Control the translational freedom in the X, Y, and Z directions respectively.

[0049]

[0050] Among them, {r1 r2 rz} respectively control the rotational freedom in the three directions of RX, RY, and RZ, n 14 represents the unit vector along the direction from target point 1 to target point 4, is the expected image n 14 The value of , ω represents the directivity coefficient.

[0051] The present invention is based on the following principle. In one embodiment of the present invention, a pinhole camera is used to normalize the target point P on the image plane coordinates. i (x i ,y i ), the image Jacobian matrix of i∈1~4.

[0052]

[0053] Among them, Z represents the depth of the point in the camera coordinate system.

[0054] Calculate the motion speed v of the robot end camera under visual servo control c :

[0055]

[0056] Where η is the controller gain, YesL s The generalized inverse matrix, L s is the image Jacobian matrix of image feature s, s is the image feature of the real-time acquired image, s * is the image feature in the desired image.

[0057] Define the image feature Jacobian matrix

[0058]

[0059] L s The calculation method is as follows:

[0060] According to the obtained four target point image features {r1 r2 r z}Construct the Jacobian matrix of the rotation direction image.

[0061]

[0062] in, Represents the image features x g ,y g ,z g The corresponding image Jacobian matrix,

[0063] L r1 ,L r2 ,L r3 Represent image features r1, r2, r z The corresponding image Jacobian matrix, p1, p2, p3, p4 represent the coordinates of the four target points in the image, L1, L2, L3, L4 represent the image Jacobian matrix of the four target points, I2 represents the second-order unit matrix, α represents the coordinates of the four target points along n 14 Directions to The unit vector of the direction.

[0064]

[0065] n 14 represents the unit vector along the direction from target point 1 to target point 4, is the expected image n 14 The value of , ω represents the direction coefficient, and I2 represents the second-order unit matrix.

[0066] According to the visual servo controller and the Jacobian matrix of the rotation direction image, the three rotation directions are adjusted in real time to ensure that the camera plane is parallel to the assembly plane. Otherwise, repeat this step.

[0067] According to the image features {x g y g z g}Construct the translation direction image Jacobian matrix.

[0068]

[0069] The three translation directions are adjusted in real time according to the visual servo controller and the Jacobian matrix of the translation direction image to realize visual servo control.

[0070] After the S2 workpiece contacts the hole, the controller processes the six-dimensional force sensor information in real time, obtains the contact force in Cartesian space, and completes the assembly according to the admittance model and control strategy.

[0071] In the world coordinate system, the various coefficients of the force sensor are calibrated by the gravity compensation method, including the zero-point values ​​F0 and M0 of the force sensor force and torque, the tool end gravity G, and the center of gravity coordinate P;

[0072] In the world coordinate system, the force sensor measures the force and torque. This value includes the force and torque formed by the gravity of the workpiece to be assembled and the contact force. The force and torque formed by the gravity of the workpiece to be assembled can be obtained through the previous calibration work, and the force and torque formed by the contact force can be obtained by making a difference, that is, the real external contact force.

[0073] The actual external contact force is used to calculate the acceleration of the end of the manipulator under force-sensing admittance control from the current moment to the next moment:

[0074]

[0075] Among them, M is the inertia matrix of the robot arm, D is the damping matrix, K is the stiffness matrix, and F ext is the actual external contact force, v is the current velocity of the end of the robot, X is the position of the end of the robot, X = [xyz] T .

[0076] Calculate the movement speed of the end of the robot under force sensor admittance control:

[0077]

[0078] Among them, v f0 It represents the speed of the robot end at the last moment, and Δt represents the time interval.

[0079] S3 calculates the movement speed v of the end of the robot arm at the next moment using the speed calculated by the image processing device and the force sensor.

[0080] The rate of establishing visual and force synergistic control is:

[0081] v=(1-λ)v c +λv f

[0082] Where v is the actual instantaneous motion of the robot, v∈R 6 , v c is the motion speed of the end of the robot under the guidance of visual servo, v f is the movement speed of the end of the robot under the control of the force sensor admittance. The value range of λ is [0,1], which indicates the weight of the robot controlled by vision and force.

[0083]

[0084] in, Z r , They represent the actual height of the robot end and the height of the robot end at the moment when the workpiece and the pin are in contact (the external contact force is not 0). ΔZ represents the height to which the end of the robot arm descends after the workpiece and the pin are in contact. Z t Indicates the height of the robot end at the assembly position. ΔZ * It indicates the total height that the end of the robot arm is expected to descend after the workpiece contacts the pin. When the external contact force is 0, λ = 0 means that the robot is controlled by visual servo. When the external contact force is not 0, it means that the workpiece has contacted the hole. At this time, the robot assembly is controlled by vision and force. And as the assembly depth increases, the proportion of force admittance control gradually increases, and the proportion of visual servo control gradually decreases to 0.

[0085] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance, characterized in that: The method comprises the following steps: The robotic arm connected with an image acquisition device and a force sensor at the end is moved to the assembly target position, and the image acquisition device acquires the image of the target point at the assembly target position as the expected image; the end of the robotic arm clamps the workpiece to be assembled and moves to the assembly target position with the workpiece to be assembled. During the movement of the end of the robotic arm, the image acquisition device acquires the image of the target point in real time, and compares the real-time acquired image with the expected image to calculate the movement speed v of the end of the robotic arm under the control of the image acquisition device c ; When the workpiece to be assembled contacts the pin at the assembly target position, the force sensor measures the force and torque at the contact, and uses the force and torque to calculate the movement speed v of the end of the robot arm under the control of the force sensor. f ; Using the robot arm movement speed v c and v f Calculate the actual movement speed v of the end of the robotic arm at the next moment.

2. A method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as claimed in claim 1, characterized in that: The movement speed v of the end of the robot arm c The calculation formula is as follows: Where η is the controller gain, YesL s The generalized inverse matrix, L s is the image Jacobian matrix of image feature s, s is the image feature of the real-time acquired image, s * is the image feature in the desired image.

3. A method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as claimed in claim 2, characterized in that: The image Jacobian matrix L of the image feature s s The formula is as follows: in, Represents the image features x g ,y g ,z g The corresponding image Jacobian matrix, L r1 ,L r2 ,L r3 Represent image features r1, r2, r z The corresponding image Jacobian matrix, p1, p2, p3, p4 represent the coordinates of the four target points in the image, L1, L2, L3, L4 represent the image Jacobian matrix of the four target points, I2 represents the second-order unit matrix, α represents the coordinates of the four target points along n 14 Directions to The unit vector of the direction.

4. A method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as claimed in claim 1 or 3, characterized in that: The 14 Directions to The formula for the unit vector α of the direction is as follows: Among them, n 14 represents the unit vector along the direction from target point 1 to target point 4, is the expected image n 14 The value of , ω represents the direction coefficient, and I2 represents the second-order unit matrix.

5. A method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as claimed in claim 1 or 4, characterized in that: The force sensor controls the movement speed v of the end of the lower robotic arm f Follow the steps below to calculate: Among them, M is the inertia matrix of the robot arm, D is the damping matrix, K is the stiffness matrix, and F ext is the actual external contact force and torque, v is the actual movement speed of the end of the robot, X is the position of the end of the robot, is the acceleration of the end of the manipulator under force-sensing admittance control, v f0 It represents the speed of the robot end at the last moment, and Δt represents the time interval.

6. A method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as claimed in claim 5, characterized in that: The actual external contact force and torque are the differences between the force and torque of the force sensor when contacting the tool end and the force and torque of the workpiece to be assembled due to gravity.

7. The method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as claimed in claim 1, characterized in that: The calculation formula of the movement speed of the end of the robotic arm at the next moment is as follows: v=(1-λ)v c +λv f Among them, v is the movement speed of the end of the robot arm at the next moment, and λ is the proportion weight.

8. The method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as claimed in claim 7, characterized in that: The calculation formula of the proportion weight λ is as follows: Among them, ΔZ represents the height to which the end of the robot arm descends after the workpiece and the pin come into contact, and ΔZ * Indicates the total height to which the end-of-arm is expected to descend after the workpiece and pin make contact.

9. A system for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance, characterized in that: The system includes an actuator, which is used to execute the method for collaboratively controlling the movement of a robotic arm based on visual servoing and force sensing admittance as described in any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for collaboratively controlling the motion of a robotic arm based on visual servoing and force sensing admittance as described in any one of claims 1 to 8 is implemented.

Images