Control method for hybrid vision and force of mechanical arm based on preset performance control

Abstract

The invention discloses a mechanical arm mixed vision and force control method based on preset performance control, and relates to the technical field of robot vision servo and force control, and the method comprises the following steps: S1, obtaining a mechanical arm kinematics model and a mechanical arm dynamics model; s2, obtaining a conversion matrix between an end effector of the mechanical arm and a camera; s3, constructing a kinematic model, and obtaining the mapping of the external torque in the feature space; s4, constructing admittance control among the feature points to realize combination of vision and force; s5, defining a visual feature error, and constraining the defined feature error through a preset performance function; s6, constructing a visual servo control law of the mechanical arm based on the obstacle Lyapunov function; and S7, the tail end speed of the mechanical arm is obtained in real time according to the visual servo control law, and motion control over the mechanical arm is achieved. According to the invention, the synchronous control of vision and force is realized, the loss of visual feature information is avoided, and the transient and steady-state response, flexibility and safety of the system are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot visual servo and force control, and in particular to a hybrid vision and force control method for a robot arm based on preset performance control. Background Art

[0002] Among many sensors, the combination of vision and force sensors is the most economical and complementary. Vision sensors provide robots with rich scene information, allowing them to operate with higher accuracy in imprecisely calibrated or dynamically changing environments. Force sensors provide more accurate contact information during interaction, making contact operations safer and more reliable.

[0003] The different data types and measurement rates of vision sensors and force sensors make this combination challenging. The prior art proposes a control strategy based on the form of a task framework to improve shared control by controlling the force along the constraint direction, and the motion in the remaining directions is controlled by vision. Another prior art proposes a hybrid vision / force control method to cope with the uncertainty of the camera and the constrained surface. This method tracks the image features at the end effector and applies orthogonal contact forces to the unknown surface. However, a disadvantage of the above two combined methods is that vision cannot correct the errors generated in the force control direction.

[0004] Traditional visual servoing usually only focuses on convergence time and steady-state error. However, the transient and steady-state responses of the visual servo system are also important performance indicators. The error can be limited by setting boundaries, thereby improving the transient and steady-state responses of the system. In one prior art, a preset performance control is introduced into the image feature error to meet the camera field of view constraint while ensuring the specified transient and steady-state performance. Designing a controller by this method is effective, but its output constraints are stricter and parameter adjustment is more difficult. Another prior art proposes a robust controller based on an obstacle Lyapunov function, which introduces a constant obstacle Lyapunov function to constrain the visual servo error. However, the symmetric constant obstacle Lyapunov function cannot achieve the required arbitrary tracking accuracy. Summary of the invention

[0005] In order to solve the problems that vision in the existing combination of vision and force cannot correct the errors generated in the force control direction, the controller parameters of the existing visual servo system are difficult to adjust, and the use of symmetric constant obstacle Lyapunov function cannot achieve the required arbitrary tracking accuracy, the present invention proposes a hybrid vision and force control method for a robotic arm based on preset performance control to solve the above problems.

[0006] The present application discloses a method for controlling a robotic arm by hybrid vision and force based on preset performance control, comprising the following steps: S1. Perform kinematic and dynamic modeling on the robot arm in the visual servo system to obtain a robot arm kinematic model and a robot arm dynamic model; S2, calibrate the visual servo system by the chessboard method to obtain the transformation matrix between the end effector of the robot arm and the camera; S3, constructing a kinematic model of visual servoing, combining the robot kinematic model and the robot dynamics model to obtain the mapping of the external torque in the feature space; S4. According to the error between the expected feature point and the compliant feature point, the admittance control between the feature points in the feature space is constructed to realize the combination of vision and force in the feature space; S5. Define a visual feature error, and constrain the defined feature error through a preset performance function according to the error between the current feature point and the compliant feature point and the field of view constraint of the camera; S6. Construct the visual servo control law of the robot arm based on the obstacle Lyapunov function; S7. Use the camera and force sensor to collect image feature point information in real time, and obtain the end speed of the robot arm in real time according to the visual servo control law, so as to inversely solve the joint speed of the robot arm, realize the motion control of the robot arm, and complete the visual servo process.

[0007] Preferably, the kinematic model of the robotic arm is:

[0008]

[0009]

[0010] in, is the position vector in the operation space, is the velocity vector in the operating space, is the acceleration vector in the operating space, is the joint position vector in the joint space, is the velocity vector in the joint space, is the acceleration vector in the joint space coordinates, is the coordinate system mapping function, is the Jacobian matrix of the robot arm.

[0011] Preferably, the mechanical arm dynamics model is:

[0012] in, is the inertia matrix of the robot arm, is the Coriolis force matrix, is the gravity vector, is the friction term, is the external torque, For control input.

[0013] Preferably, S2 comprises the following steps: S21, use a chessboard calibration plate with N points, randomly placed in the field of view of the RealSense camera; S22, using the RealSense camera to collect images of the chessboard calibration plate; S23, calculating the center pixel coordinates of N dots in the image; S24, recording the three-dimensional position and posture of the end effector of the robot arm at this time in the form of a translation vector and a rotation vector; S25, repeat S220-S240 18 times in total; S26. Solve 2D-3D data and find the coordinate transformation relationship between the chessboard calibration plate and the camera ; S27, according to the coordinate conversion relationship , calculate the transformation matrix from the end effector of the robot to the camera .

[0014] Preferably, the kinematic model of the visual servoing is:

[0015] in, is the image feature point velocity, is the image interaction matrix, is the camera movement speed.

[0016] Preferably, the mapping of the external moment in the feature space is:

[0017]

[0018] in, is the external contact force in the image feature space, is the image interaction matrix, is the inverse of the projection of the robot arm inertia matrix into the camera frame, is the force in the camera coordinate system, Represents matrix transpose.

[0019] Preferably, the admittance control between the feature points in the feature space is:

[0020]

[0021] in, is the virtual mass, For damping, is the stiffness, is the expected feature point set initially, A compliant feature point that is moved by an external contact force.

[0022] Preferably, S5 comprises the following steps: S51. Define the visual feature error as:

[0023]

[0024] in, is the current feature point; S52, determining the camera's field of view constraint according to the camera resolution as follows:

[0025] in, The feature point is The image plane coordinates on the axis, The feature point is The image plane coordinates on the axis, for The lower bound of the axis in image plane coordinates, for The upper bound of the axis in image plane coordinates, for The lower bound of the axis in image plane coordinates, for The upper bound of the axis in image plane coordinates, For time; S53, combined with the visual feature error in S1, the error constraint is obtained as:

[0026] in, for The expected value of the feature point on the axis, for Expected value of feature points on the axis; for Pixel error on axis, for Pixel error on axis; S54, constraining the visual feature error in S51 by a preset performance function, wherein the preset performance function is shown in the following formula:

[0027]

[0028] in, for An exponential decay function on the axis, for An exponential decay function on the axis, To adjust the parameters of the error convergence rate, is the parameter that affects the size of the convergence error. Can be set to any small value that satisfies ; , , , , , , and is a positive parameter under the camera field of view constraint; , , , .

[0029] Preferably, S6 comprises the following steps: S61. Selecting the barrier Lyapunov function :

[0030] in, , , and for the boundaries of; S62. Derivative of the obstacle Lyapunov function in S61:

[0031] in, , ; S63. In order to stabilize the visual servo system, the visual servo control law of the robot arm is constructed:

[0032]

[0033] in, is the camera's movement speed, is the speed of feature point error, is the controller gain, is the pseudo-inverse of the image interaction matrix.

[0034]

[0035]

[0036]

[0037]

[0038]

[0039]

[0040]

[0041]

[0042] in, , , , and To construct a vector.

[0043] Beneficial effects of the present invention: (1) The present invention couples the visual sensor and the force sensor in the feature space, solves the problem of inconsistent driving layers of the force sensor and the visual sensor, and realizes synchronous control of vision and force; (2) The present invention introduces an asymmetric time-varying tan-type obstacle Lyapunov function into the visual servo system. The constructed controller limits the image feature error within the generated constraint boundary and satisfies the camera's field of view constraint. Compared with the constant time-invariant obstacle Lyapunov function, the desired arbitrary tracking accuracy is achieved. (3) The preset performance function specified in the present invention is a priori and designable, has low functional complexity, and is easy to apply. When the same preset performance function is selected, the barrier Lyapunov function requires fewer restrictions and its parameters are easy to adjust. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 This is a flow chart of a method for controlling a robot arm hybrid vision and force based on preset performance control according to an embodiment of the present invention; Figure 2 The characteristic point error in the image plane under the condition of no contact force in the embodiment of the present invention is Transient and steady-state response plots on axis; Figure 3 The characteristic point error in the image plane under the condition of no contact force in the embodiment of the present invention is Transient and steady-state response plots on axis; Figure 4 The transient and steady-state response diagrams of the total feature point error in the case of no contact force according to an embodiment of the present invention; Figure 5A curve diagram of feature point errors of a hybrid vision and force control method in the case of contact force according to an embodiment of the present invention; Figure 6 This is a curve diagram of the feature point error of the pure visual control method in the case of contact force according to an embodiment of the present invention. DETAILED DESCRIPTION

[0045] In order to make the objectives, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below with reference to the accompanying drawings and examples.

[0046] The present application embodiment discloses a method for controlling a robot arm by hybrid vision and force based on preset performance control. The method flow is as follows: Figure 1 As shown, the following steps are included: S1. Perform kinematic and dynamic modeling on the robot arm in the visual servo system to obtain the robot arm kinematic model and the robot arm dynamic model, and set the coordinate system of the visual servo system reference frame.

[0047] The kinematic model of the robotic arm is:

[0048]

[0049]

[0050] in, is the position vector in the operation space, is the velocity vector in the operating space, is the acceleration vector in the operating space, is the joint position vector in the joint space, is the velocity vector in the joint space, is the acceleration vector in the joint space coordinates, is the coordinate system mapping function, is the Jacobian matrix of the robot arm.

[0051] The dynamic model of the robotic arm is:

[0052] in, is the inertia matrix of the robot arm, is the Coriolis force matrix, is the gravity vector, is the friction term, is the external torque, For control input.

[0053] S2. In this embodiment, the visual servo system includes a six-degree-of-freedom UR5 collaborative robot, a six-axis FT300 force / torque sensor, an Intel RealSense D435i camera, a computer, a 36h11 series AprilTag QR code, and processed mechanical parts. The visual servo system is calibrated by the chessboard method to obtain the transformation matrix between the end effector of the robot and the camera.

[0054] S21. Use a chessboard calibration plate with N points and place it randomly in the field of view of the RealSense camera.

[0055] S22. Use the RealSense camera to collect images of the chessboard calibration plate.

[0056] S23, calculating the center pixel coordinates of N dots in the image.

[0057] S24. Record the three-dimensional position and posture of the end effector of the robot arm in the form of a translation vector and a rotation vector.

[0058] S25. Repeat S220-S240 a total of 18 times.

[0059] S26. Solve 2D-3D data and find the coordinate transformation relationship between the chessboard calibration plate and the camera .

[0060] S27, according to the coordinate conversion relationship , calculate the transformation matrix from the end effector of the robot to the camera .

[0061] S3. Construct a kinematic model of visual servoing, and combine the robot kinematic model and the robot dynamics model to obtain the mapping of the external torque in the feature space.

[0062] The kinematic model of visual servoing is:

[0063] in, is the image feature point velocity, is the image interaction matrix, is the camera movement speed.

[0064] The mapping of external torque in the characteristic space is:

[0065]

[0066] in, is the external contact force in the image feature space, is the image interaction matrix, is the inverse of the projection of the robot arm inertia matrix into the camera frame, is the force in the camera coordinate system, Represents matrix transpose.

[0067] S4. According to the error between the expected feature point and the compliant feature point, the admittance control between the feature points in the feature space is constructed to realize the combination of vision and force in the feature space.

[0068] The admittance control between feature points in the feature space is:

[0069]

[0070] in, is the virtual mass, For damping, is the stiffness, is the expected feature point set initially, A compliant feature point that is moved by an external contact force.

[0071] S5. Define a visual feature error, and constrain the defined visual feature error through a preset performance function according to the error between the current feature point and the compliant feature point and the field of view constraint of the camera.

[0072] S51. Define the visual feature error as:

[0073]

[0074] in, is the current feature point; S52, determining the camera's field of view constraint according to the camera resolution as follows:

[0075] in, The feature point is The image plane coordinates on the axis, The feature point is The image plane coordinates on the axis, for The lower bound of the axis in image plane coordinates, for The upper bound of the axis in image plane coordinates, for The lower bound of the axis in image plane coordinates, for The upper bound of the axis in image plane coordinates, For time; S53, combined with the visual feature error in S1, the error constraint is obtained as:

[0076] in, for The expected value of the feature point on the axis, for Expected value of feature points on the axis; for Pixel error on axis, for Pixel error on axis; S54, constraining the visual feature error in S51 by a preset performance function, wherein the preset performance function is shown in the following formula:

[0077]

[0078] in, for An exponential decay function on the axis, for An exponential decay function on the axis, To adjust the parameters of the error convergence rate, is the parameter that affects the size of the convergence error. Can be set to any small value that satisfies ; , , , , , , and is a positive parameter under the camera field of view constraint; , , , .

[0079] S6. Construct the visual servo control law of the robotic arm based on the obstacle Lyapunov function.

[0080] S61. Selecting the barrier Lyapunov function :

[0081] in, , , and for the boundaries of; S62. Derivative of the obstacle Lyapunov function in S61:

[0082] in, , ; S63. In order to stabilize the visual servo system, the visual servo control law of the robot arm is constructed:

[0083]

[0084] in, is the camera's movement speed, is the speed of feature point error, is the controller gain, is the pseudo-inverse of the image interaction matrix. , , and To construct a vector.

[0085]

[0086]

[0087]

[0088]

[0089]

[0090]

[0091]

[0092]

[0093] in, .

[0094] S7. Use the camera and force sensor to collect image feature point information in real time, and obtain the end speed of the robot arm in real time according to the visual servo control law, so as to inversely solve the joint speed of the robot arm, realize the motion control of the robot arm, and complete the visual servo process.

[0095] Specifically, programming is performed on the PC side according to the designed visual servo controller, and the manipulator, torque sensor, and Visp platform communicate through ROS. The embodiment is based on the background of overhead line bolt maintenance, and aims to realize the full automation of overhead line intelligent maintenance and improve the efficiency and safety of the overhead line maintenance process.

[0096] In a specific embodiment, in the absence of external contact force, the camera identifies the maintenance target and visual servoing moves to the maintenance position.

[0097] The test results are as follows Figure 2-Figure 4 As shown, Figure 2 and Figure 3 They are the feature point errors in the image plane Axis and Transient and steady-state response plots on the axis, Figure 4 It is the transient and steady-state response diagram of the total feature point error. Among them, the red, green, cyan and blue lines represent the control methods of asymmetric log-type preset performance, asymmetric log-type preset performance, symmetric preset performance combined with log-type obstacle Lyapunov and asymmetric preset performance combined with tan-type obstacle Lyapunov of the present invention, respectively, and the dotted line represents the preset performance function. It can be seen from the figure that the present invention has better transient and steady-state response under the constraint of the camera field of view, and the error convergence time is the shortest.

[0098] In another specific embodiment, in the presence of an external contact force, a camera identifies a maintenance target and visual servoing moves to the maintenance position.

[0099] The test results are as follows Figure 5 and Figure 6 As shown, Figure 5 and Figure 6 They are a hybrid vision and force control method for the robot arm based on preset performance control and a pure vision control method. Figure 5 It can be seen from the figure that the present invention automatically adjusts the force compliance when the first hole test fails. At this time, the system performs the hole test under the simultaneous action of vision and force and successfully completes the task. Figure 6 It can be seen that pure visual servo control without force compliance will not make adjustments when the above situation occurs. Due to the existence of the end effector, when the characteristic error of the visual servo has not converged, the end effector will continue to squeeze the workpiece. Since the reaction force continues to act on the manipulator, it will eventually cause the manipulator to stop protectively, causing the manipulator to vibrate. The present invention can perform adaptive adjustment when performing hole assembly, has better flexibility and safety, and improves the success rate of assembly tasks.

[0100] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A hybrid vision and force control method for a robotic arm based on preset performance control, It is characterized in that The following steps are involved: S1. Perform kinematic and dynamic modeling on the robot arm in the visual servo system to obtain a robot arm kinematic model and a robot arm dynamic model; S2, calibrate the visual servo system by the chessboard method to obtain the transformation matrix between the end effector of the robot arm and the camera; S3, constructing a kinematic model of visual servoing, combining the robot kinematic model and the robot dynamics model to obtain the mapping of the external torque in the feature space; S4. According to the error between the expected feature point and the compliant feature point, the admittance control between the feature points in the feature space is constructed to realize the combination of vision and force in the feature space; S5. Define a visual feature error, and constrain the defined feature error through a preset performance function according to the error between the current feature point and the compliant feature point and the field of view constraint of the camera; S6. Construct the visual servo control law of the robot arm based on the obstacle Lyapunov function; S7. Use the camera and force sensor to collect image feature point information in real time, and obtain the end speed of the robot arm in real time according to the visual servo control law, so as to inversely solve the joint speed of the robot arm, realize the motion control of the robot arm, and complete the visual servo process.

2. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 1, It is characterized in that The kinematic model of the robot arm is: in, is the position vector in the operation space, is the velocity vector in the operating space, is the acceleration vector in the operating space, is the joint position vector in the joint space, is the velocity vector in the joint space, is the acceleration vector in the joint space coordinates, is the coordinate system mapping function, is the Jacobian matrix of the robot arm.

3. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 2, It is characterized in that The mechanical arm dynamics model is: in, is the inertia matrix of the robot arm, is the Coriolis force matrix, is the gravity vector, is the friction term, is the external torque, For control input.

4. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 3, It is characterized in that The S2 comprises the following steps: The 2D-3D data obtained by the chessboard method is calibrated to find the coordinate transformation relationship between the chessboard calibration plate and the camera , and calculate the transformation matrix from the robot end effector to the camera .

5. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 4, It is characterized in that The kinematic model of the visual servo is: in, is the image feature point velocity, is the image interaction matrix, is the camera movement speed.

6. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 5, It is characterized in that The mapping of the external moment in the characteristic space is: in, is the external contact force in the image feature space, is the image interaction matrix, is the inverse of the projection of the robot arm inertia matrix into the camera frame, is the force in the camera coordinate system, Represents matrix transpose.

7. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 6, It is characterized in that The admittance control between the feature points in the feature space is: in, is the virtual mass, For damping, is the stiffness, is the expected feature point set initially, A compliant feature point that is moved by an external contact force.

8. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 7, It is characterized in that The S5 comprises the following steps: S51. Define the visual feature error as: in, is the current feature point; S52, determining the camera's field of view constraint according to the camera resolution as follows: in, The feature point is The image plane coordinates on the axis, The feature point is The image plane coordinates on the axis, for The lower bound of the axis in image plane coordinates, for The upper bound of the axis in image plane coordinates, for The lower bound of the axis in image plane coordinates, for The upper bound of the axis in image plane coordinates, For time; S53, combined with the visual feature error in S1, the error constraint is obtained as: in, for The expected value of the feature point on the axis, for Expected value of feature points on the axis; for Pixel error on axis, for Pixel error on axis; S54, constraining the visual feature error in S51 by a preset performance function, wherein the preset performance function is shown in the following formula: in, for An exponential decay function on the axis, for An exponential decay function on the axis, To adjust the parameters of the error convergence rate, is the parameter that affects the size of the convergence error. Can be set to any small value that satisfies ; , , , , , , and is a positive parameter under the camera field of view constraint; , , , .

9. The method for controlling a robot arm by hybrid vision and force based on preset performance control according to claim 8, It is characterized in that The S6 comprises the following steps: S61. Selecting the barrier Lyapunov function : in, , , and for the boundaries of; S62. Derivative of the obstacle Lyapunov function in S61: in, , ; S63. In order to stabilize the visual servo system, the visual servo control law of the robot arm is constructed: in, is the camera's movement speed, is the speed of feature point error, is the controller gain, is the pseudo-inverse of the image interaction matrix; in, , , , and To construct a vector.

Images