CN118721200A - Visual servo control method, device and storage medium for dual-arm collaborative robot - Google Patents

Abstract

The present invention provides a dual-arm collaborative robot visual servo control method, device and storage medium, which relates to the field of mechanical automation technology, including: obtaining image information of a binocular vision system; establishing a coordinate relationship between a robotic arm and a target object based on camera calibration and hand-eye calibration; performing static binocular vision calibration only once at the initial posture, and then obtaining the spatial three-dimensional coordinates of the target object through a coordinate system mapping relationship, thereby realizing dynamic binocular vision; identifying the target object based on an improved YOLO target detection algorithm; determining the target object position information according to the result of target detection; determining the master-slave relationship between a first robotic arm and a second robotic arm according to the position information; controlling the main arm to reach a grasping posture, and controlling the slave arm to drive a camera to an observation position according to the posture of the main arm; performing optimal posture estimation on the target object and controlling the main arm to accurately grasp it. The present invention improves the robustness and accuracy of the dual-arm collaborative robot in a dynamic environment.

Description

Visual servo control method, device and storage medium for dual-arm collaborative robot

Technical Field

The present invention relates to the field of mechanical automation technology, and in particular to a visual servo control method, device and storage medium for a dual-arm collaborative robot.

Background Art

Due to the diversity of robot working environments and the complexity of tasks, single-arm robots are often unable to perform complex tasks due to the limitations of the working environment and their own conditions. Compared with single-arm robots, dual-arm robots are not only more flexible and more functional, but also more adaptable to changes in the working environment and capable of handling a variety of working environments. Dual-arm robots are not simply a pile of single-arm robots in terms of quantity, but rather use effective collaborative control mechanisms to complete work tasks together. Since the advent of the first dual-arm robot, dual-arm robots have been defined as a robot system with high flexibility and collaborative capabilities, gradually becoming the hottest research topic in the field of robotics, and also a major trend in the future development of robots.

In order to enhance the robot's ability to accurately identify targets and make correct judgments in complex and changing environments, the existing technology integrates various sensors in the robot. Among the many sensors of the robot, the visual sensor is the most critical one because it provides real-time information about the surrounding environment, enabling the robot to perceive and understand the shape, position and characteristics of objects. According to the position of the camera and the robot, it can be divided into "Eye-in-hand (abbreviated as EIH)" camera and "Eye-to-hand (abbreviated as ETH)" camera. Robots based on visual sensors can interact with the environment more effectively and perform a variety of tasks. The integration of visual technology makes robots more intelligent, able to adapt to more complex and changing working environments, and able to collaborate more harmoniously with humans and machines to complete complex and diverse work tasks together. Therefore, visual technology has become a key technology in robot applications, and visual sensors have also become a key component in realizing robot intelligence.

The premise for a multi-vision system to measure an object is to know the internal parameters of the camera and the position relationship between the two cameras. Therefore, the camera position in the multi-vision system will not change after calibration, so the general multi-vision system is static. If the position between the two cameras changes during the execution of the task, recalibration is required, otherwise the task will not be completed. Therefore, in a complex and changing dynamic environment, the fixed multi-vision system is greatly limited, which makes the robot inaccurate and unstable when performing tasks.

Therefore, how to improve the robustness and accuracy of dual-arm collaborative robots in dynamic environments has become a technical problem that needs to be solved urgently.

Summary of the invention

The present invention aims to solve at least one of the technical problems existing in the prior art or related technology, and discloses a visual servo control method, device and storage medium for a dual-arm collaborative robot, which improves the robustness and accuracy of the dual-arm collaborative robot in a dynamic environment.

The first aspect of the present invention discloses a dual-arm collaborative robot visual servo control method, comprising: acquiring image information of a target object collected by a first camera arranged at the end of a first robotic arm and a second camera arranged at the end of a second robotic arm; establishing a coordinate relationship between the robotic arm and the target object based on camera calibration and hand-eye calibration; performing static binocular vision calibration only once at an initial position, and then obtaining the spatial three-dimensional coordinates of the target object through a coordinate system mapping relationship, thereby realizing dynamic binocular vision; identifying the target object based on an improved YOLO target detection algorithm, wherein the improved YOLO target detection algorithm is based on YOLO-v5 The ECA attention mechanism and the SA attention mechanism are added to the s model; the position information of the target object is determined according to the result of target detection; the master-slave relationship of the first robotic arm and the second robotic arm is determined according to the position information; the master arm is controlled to reach the grasping position, and the slave arm is controlled to drive the camera to the observation position according to the position of the master arm; the position-based visual servo control method is adopted, combined with the EK-SVSF pose estimation algorithm and the step-by-step data fusion algorithm based on OWA to estimate the optimal pose of the target object; according to the result of the optimal pose estimation, the main arm is controlled to adjust the deviation through joint movement, so as to achieve accurate grasping of the target object.

According to the visual servo control method for a dual-arm collaborative robot disclosed in the present invention, preferably, the first camera and the second camera are depth cameras, which follow the movement of the free ends of the first robotic arm and the second robotic arm respectively.

According to the dual-arm collaborative robot visual servo control method disclosed in the present invention, preferably, the calculation process of camera calibration includes: fixing the camera and continuously collecting calibration plate images; calculating the pixel coordinates of the corner points; calculating the homography matrix; solving the intrinsic parameter matrix; solving the extrinsic parameter matrix; solving the lens distortion coefficient; based on the minimum reprojection error function, using maximum likelihood estimation to iterate and optimize all parameters obtained by the camera calibration.

According to the dual-arm collaborative robot visual servo control method disclosed in the present invention, preferably, the calculation process of hand-eye calibration includes: calculating the mapping matrix from the robot arm end coordinate system to the base coordinate system; calculating the mapping matrix from the world coordinate system to the camera coordinate system; calculating the mapping matrix from the camera coordinate system to the robot arm end coordinate system.

According to the dual-arm collaborative robot visual servo control method disclosed in the present invention, preferably, the calculation process of dynamic binocular vision includes: calculating the mapping relationship between the first camera and the base coordinate system in the initial posture; calculating the mapping relationship between the base coordinate system and the first camera in the nth posture; calculating the mapping relationship between the world coordinate system and the base coordinate system; calculating the mapping relationship between the world coordinate system and the first camera in the nth posture; calculating the mapping relationship between the world coordinate system and the second camera in the nth posture; calculating the mapping relationship between the second camera and the first camera in the nth posture; calculating the dynamic baseline in the nth posture according to the relative translation vector of the first camera and the second camera; obtaining depth information according to the dynamic baseline and the principle of triangulation; obtaining the three-dimensional coordinates of the spatial point according to the depth information and the projection information of the target point on the image plane of the second camera, and using the camera intrinsic parameters and the projection relationship.

According to the dual-arm collaborative robot visual servo control method disclosed in the present invention, preferably, the improved YOLO target detection algorithm specifically includes: adding an ECA module and a SA module before each convolution block of the Neck layer of the YOLO-v5s model.

According to the dual-arm collaborative robot visual servo control method disclosed in the present invention, preferably, it also includes: replacing the nearest neighbor interpolation method of the upsampling module of the YOLO-v5s model with a deconvolution method, with a convolution kernel size of 4 and a convolution step size of 2, and back-propagating gradients through learning parameters to map the low-resolution feature map back to the high-resolution space.

According to the dual-arm collaborative robot visual servo control method disclosed in the present invention, preferably, the step of determining the master-slave relationship between the first robotic arm and the second robotic arm specifically includes: determining the center point of the target object through a visual system; calculating the Euclidean distance Ll between the center point of the target object and the first robotic arm, and calculating the Euclidean distance Lr between the center point of the target object and the second robotic arm; when Ll＜Lr, selecting the first robotic arm as the master arm and the second robotic arm as the slave arm; when Ll＞Lr, selecting the second robotic arm as the master arm and the first robotic arm as the slave arm.

The second aspect of the present invention discloses a dual-arm collaborative robot visual servo control device, comprising: a memory for storing program instructions; a processor for calling the program instructions stored in the memory to implement a dual-arm collaborative robot visual servo control method such as any of the above technical solutions.

A third aspect of the present invention discloses a computer-readable storage medium storing a program code, wherein the program code is used to implement a visual servo control method for a dual-arm collaborative robot as described in any of the above technical solutions.

The beneficial effects of the present invention include at least: a dynamic binocular vision calibration method with stronger adaptability to environmental changes is proposed, target positioning is achieved through the traditional binocular vision calibration method and the coordinate system mapping relationship, and strong robustness and accuracy are shown in a dynamic environment; in view of the problems of long inference time and easy loss of image information of YOLO-v5s in a dynamic environment, an improved YOLO target detection algorithm is proposed, which introduces ECA module and SA module in the Neck network, and replaces the nearest neighbor interpolation method in upsampling with the deconvolution method, which effectively improves the target detection accuracy; a position-based master-slave planning strategy is proposed, and a position-based visual servo control method is adopted, which combines the EK-SVSF pose estimation algorithm and the OWA-based step-by-step data fusion algorithm to estimate the optimal pose of the target object, thereby improving the grasping accuracy of the manipulator.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 shows a schematic diagram of the workflow of a dual-arm collaborative robot visual servo control method according to an embodiment of the present invention.

FIG. 2 is a schematic diagram showing a coordinate system of a camera imaging process according to an embodiment of the present invention.

FIG. 3 is a schematic diagram showing a mapping process from a world coordinate system to a pixel coordinate system according to an embodiment of the present invention.

FIG. 4 shows a schematic diagram of hand-eye calibration according to an embodiment of the present invention.

FIG5 shows a schematic diagram of a dynamic binocular vision calibration model according to an embodiment of the present invention.

FIG. 6 is a schematic diagram showing the triangulation principle according to an embodiment of the present invention.

FIG7 shows a schematic diagram of a network model of an improved YOLO target detection algorithm according to an embodiment of the present invention.

FIG. 8 is a schematic flow chart of a position-based visual servo control method according to an embodiment of the present invention.

FIG. 9 shows a schematic diagram of an OWA distributed data fusion process according to an embodiment of the present invention.

FIG. 10 shows a schematic block diagram of a visual servo control device for a dual-arm collaborative robot according to an embodiment of the present invention.

DETAILED DESCRIPTION

In order to more clearly understand the above-mentioned objects, features and advantages of the present invention, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the present invention is not limited to the specific embodiments disclosed below.

According to one embodiment of the present invention, the visual servo control method of a dual-arm collaborative robot disclosed in the present invention comprises: acquiring image information of a target object collected by a first camera arranged at the end of a first robotic arm and a second camera arranged at the end of a second robotic arm; establishing a coordinate relationship between the robotic arm and the target object based on camera calibration and hand-eye calibration; performing static binocular vision calibration only once at an initial position, and then obtaining the spatial three-dimensional coordinates of the target object through a coordinate system mapping relationship, thereby realizing dynamic binocular vision; identifying the target object based on an improved YOLO target detection algorithm, wherein the improved YOLO target detection algorithm is based on the YOLO- The ECA attention mechanism and the SA attention mechanism are added to the v5s model; the target position information is determined according to the result of target detection; the master-slave relationship between the first robotic arm and the second robotic arm is determined according to the position information; the master arm is controlled to reach the grasping position, and the slave arm is controlled to drive the camera to the observation position according to the position of the master arm; the position-based visual servo control method is adopted, combined with the EK-SVSF pose estimation algorithm and the OWA-based step-by-step data fusion algorithm to estimate the optimal pose of the target; according to the result of the optimal pose estimation, the main arm is controlled to adjust the deviation through joint movement, thereby achieving accurate grasping of the target.

In this embodiment, as shown in FIG1 , the dual-arm collaborative robot visual servo control method of the present invention mainly includes three stages: (1) Initial stage: some preparation work needs to be done before the entire control system is operated, including calibration and parameter configuration of the visual system. The calibration of the visual system includes camera calibration, hand-eye calibration and binocular vision calibration; the parameter configuration includes camera internal parameters, mechanical arm structure parameters and motion parameters, communication parameters between each component and the host, etc. (2) Identification and positioning stage: the premise of controlling the mechanical arm to successfully grasp the target is to find the target, so it is necessary to control the camera to collect the target image, the host recognizes the target through the target detection algorithm, and obtains the three-dimensional coordinates of the center point of the target through binocular vision calibration. (3) Visual servo control stage: the master-slave relationship of the two mechanical arms is determined by the master-slave planning strategy based on position, and the servo system control method based on position is used to drive the dual-arm collaborative robot to move. The master and slave eyes collect the target image, estimate the position and pose of the target, and obtain the optimal position and pose through the data fusion algorithm, so as to control the mechanical arm to grasp accurately. In general, the camera identifies and locates the target object, and transmits its position information to the main control system; the main control system determines the master-slave relationship between the left and right robot arm bases and the target object based on the distance between the two; the main control system controls the main arm to drive the camera mounted on the end of the main arm to approach the target object for pose estimation, and at the same time, the slave arm drives the camera mounted on the end of the slave arm into the optimal observation pose according to the pose of the main arm, completing the pose estimation of the main arm and the target object; the two pose estimation results are transmitted back to the main control system for data fusion to obtain the optimal pose of the target object, thereby controlling the main arm for precise grasping.

According to the above embodiment, preferably, the dual-arm system uses the RM65 ultra-light six-axis robotic arm, equipped with an EG2-4C two-finger electric gripper, the visual system uses a RealSenseD435 camera, and the main control system uses Nvidia Jetson Nano; the software system framework is built based on ROS.

According to the above embodiment, the specific process and principle of camera calibration include: the camera imaging process is to convert three-dimensional space points into two-dimensional plane points, and camera calibration is to reversely solve the camera imaging model parameters. These parameters include internal parameters related to the internal structure of the camera, such as the principal point and focal length, and external parameters related to the position of the camera, such as rotation and translation. The camera imaging process involves the mutual conversion relationship of four coordinate systems. As shown in Figure 2, the four coordinate systems are the pixel coordinate system o-uv, the image coordinate system O-xy, the camera coordinate system Oc-XcYcZc and the world coordinate system Ow-XwYwZw. P is a point in the actual space, and its imaging point on the camera image is p(x, y). The distance between Oc and O is the camera focal length f. The process of solving the camera imaging model parameters can be understood as a mapping process from the world coordinate system to the pixel coordinate system. As shown in Figure 3, the process of solving the camera imaging model parameters includes three parts: rigid body transformation, perspective projection and affine transformation. The mapping process can be expressed by formula (3.1):

Where (u, v) is the pixel coordinate, A is the intrinsic parameter matrix of the camera calibration, including the focal length f, the image principal point coordinates (u0, v0) and the distortion coefficient, T is the extrinsic parameter matrix of the camera calibration, including the rotation matrix R and the translation vector t, dX and dY are the actual physical lengths of the unit pixel in the X and Y directions of the camera plate, and θ is the angle between the horizontal and vertical edges of the camera plate, which is ideally 90°.

However, in the actual imaging process, due to physical factors in the manufacturing or use of the lens, the actual working principle of the camera deviates from the ideal pinhole imaging model. Therefore, the shape of the object is distorted or stretched during the imaging process, affecting the true expression of the geometric shape and size of the image, that is, lens distortion: Assuming that (x, y) is the image coordinate with distortion and (x, y) is the image coordinate without distortion, the mathematical model of radial distortion is shown in formula (3.2):

Wherein, r represents the distance from the image pixel to the image center, that is, r2=x2+y2, and k1, k2 and k3 are mirror distortion coefficients.

The mathematical model of tangential distortion is shown in formula (3.3):

Among them, p1 and p2 are the tangential distortion coefficients.

The present invention adopts Zhang Zhengyou camera calibration method to perform camera calibration:

In order to make the various camera parameters as close to the accurate values as possible, this method usually uses linear modeling to find their initial values, and then uses nonlinear modeling to correct the initial values to obtain parameters with lower errors.

The specific steps of Zhang Zhengyou's camera calibration are as follows:

(1) Image acquisition: The camera is fixed and continuously acquires images of the calibration plate. To ensure data diversity and accuracy, it is necessary to ensure that the calibration plate is within the camera acquisition range. At the same time, the distance, angle, and orientation between the calibration plate and the camera need to be constantly changed during the acquisition process.

(2) Calculate the pixel coordinates of corner points: Use the image detection algorithm to calculate the pixel coordinates of each corner point on the calibration plate.

(3) Calculate the homography matrix M: The homography matrix in Zhang Zhengyou's camera calibration is the product of the intrinsic parameter matrix A and the extrinsic parameter matrix T. Assuming that the X _w axis and Y _w axis of the world coordinate system are parallel to the plane of the calibration plate, and the Z _w axis is perpendicular to the plane of the calibration plate, the Z value of the entire calibration plate in the world coordinate system is 0. Therefore, equation (3.1) can be rewritten as equation (3.4).

Eliminating the scale factor Z _c , we can obtain the mapping relationship from the world coordinate system to the pixel coordinate system, as shown in equation (3.5).

Theoretically, the homography matrix M can be obtained from four corner points. However, due to the influence of factors such as noise, the least squares method is generally used to estimate the homography matrix for as many corner points as possible to obtain the optimal homography matrix M.

(4) Find the intrinsic parameter matrix A: In Zhang Zhengyou's calibration algorithm, formula (3.6) is generally used as the intrinsic parameter matrix for camera calibration. Among them, γ is the distortion parameter, α and β are the number of pixels per unit distance, and (u ₀ ,v ₀ ) is the coordinate of the image principal point.

Let B = A-TA-1, as shown in formula (3.7), it can be seen that matrix B is a symmetric matrix with only 6 unknown quantities. Therefore, theoretically, matrix B can be obtained by three calibration plate images.

However, in order to improve the stability and accuracy of the calibration results and obtain a more reliable camera intrinsic parameter matrix, the least squares method is generally used to fit as many calibration plate images as possible. Using the matrix decomposition algorithm, A and A ^-1 are obtained. The expression of the unknown quantity in A is shown in formula (3.8).

(5) Obtaining the extrinsic parameter matrix: Based on A, we can obtain the extrinsic parameter matrix of the camera calibration, as shown in equation (3.9). Where, λ = 1/‖A ^-1 M ₁ ‖ = 1/‖A ^-1 M ₂ ‖,

[R t]＝[R ₁ R ₂ R ₃ t]＝[λA ^-1 M ₁ λA ^-1 M ₂ R ₁ ×R ₂ λA ^-1 M ₃ ](3.9)

(6) Calculate the lens distortion coefficient: Low-order radial distortion is added to Zhang Zhengyou’s calibration method. The matrix form of the model is shown in equation (3.10), where (u^,^v) is the pixel coordinate with distortion.

Formula (3.10) is simplified as Dk=d, and the distortion coefficient is obtained by using the least squares method, as shown in formula (3.11).

k＝(D ^T D) ^-1 D ^T d (3.11)

(7) Optimizing parameters: All parameters obtained by camera calibration are optimized by iterating the reprojection error minimization function of equation (3.12) using maximum likelihood estimation ^[54] , where is the pixel coordinate corresponding to the corner point _Mij .

As shown in FIG4 , according to the above embodiment, the specific process and principle of hand-eye calibration include: the present invention adopts an EIH hand-eye system, and the robotic arm moves to two different postures to calibrate the calibration plate, and ensures that the calibration plate is within the field of view of the camera.

In the EIH hand-eye system, the Camera is fixed relative to the End and changes relative to the Base, so the quantity to be determined is the mapping matrix from Camera to End, represented by ^e T _c . In addition, the mapping matrix from End to Base is represented by ^b T _e ; the mapping matrix from World to Camera is represented by ^c T _w ; and the mapping matrix from World to Base is represented by ^b T _w .

Assume that P is a point in space, and its coordinates in Base, World, Camera, and End are _Pb ( _Xb , _Yb , _Zb ), _Pw ( _Xw , _Yw , _Zw ), _Pc ( _Xc , _Yc , _Zc ), and _Pe ( _Xe , _Ye , _Ze ), respectively. The process _of solving ^eTc is as follows:

(1) End to Base mapping matrix ^b T _e

P _e can be obtained by rigid transformation to P _b , as shown in equation (3.13), where the rotation matrix ^b _Re = R _z R _y R _x and the translation vector ^b t _e can be obtained by the forward kinematics of the robot arm.

P _b ＝ ^b R _e P _e + ^b t _e (3.13)

Convert equation (3.13) into matrix form, as shown in equation (3.14).

Then the conversion relationship from _Pe to _Pb can be expressed by formula (3.15), and _the mapping matrix ^bTe from End to Base can be expressed by (3.16).

(2) World to Camera mapping matrix ^c T _w

P _w can be obtained by the external parameter matrix of camera calibration to obtain P _c , as shown in formula (3.17).

P _c = ^c R _w P _w + ^c t _w (3.17)

Among them, ^c R _w and ^c t _w are the rotation matrix and translation vector of P _w relative to P _c , respectively.

Similarly, converting equation (3.17) into matrix form can obtain the transformation relationship between _Pw and _Pc , as shown in equation (3.18), and _the mapping matrix ^cTw from World to Camera can be expressed by equation (3.19).

P _c ＝ ^c T _w P _w (3.18)

(3) Camera to End mapping matrix ^e T _c

Let the conversion relationship between _Pc and _Pe be equation (3.20), and the conversion relationship between _Pw and _Pb be equation (3.21).

_{Pe =} ^eTcPc ( _3.20 )

P _b ＝ ^b T _w P _w (3.21)

Combining equations (3.15), (3.18) and (3.20) we can get equation (3.22).

P _b ＝ ^b T _e P _e ＝ ^b T _e ^e T _c P _c ＝ ^b T _e ^e T _c ^c T _w P _w (3.22)

According to equations (3.21) and (3.22), the relationship between the mapping matrices can be obtained, as shown in equation (3.23).

^b T _w ＝ ^b T _e ^e T _c ^c T _w (3.23)

Because the relative positions of the base and the calibration plate remain unchanged, and the relative positions of the gripper and the camera remain unchanged, ^b T _w and ^e T _c are both constants in equation (3.23). Under the premise of ensuring that the calibration plate is within the camera's field of view, the robot moves to two different positions to obtain equation (3.24).

Formula (3.24) is a typical "AX = XB" problem, where X = ^e T _c is the transformation matrix to be determined, It can be obtained according to the robot's forward kinematics, It can be obtained based on the external parameter matrix of camera calibration. Using the method proposed by Tsai, the transformation matrix ^e T _c of Camera relative to End can be solved, as shown in formula (3.25), which is the result of hand-eye calibration.

As shown in FIG5 , according to the above embodiment, the present invention also discloses an algorithm for realizing dynamic binocular vision:

The dynamic binocular vision calibration model is shown in Figure 5. The pose before the camera moves is called the initial pose, and a certain pose during the camera movement is called the nth pose. is the optical center of the left and right cameras in the initial position, is the optical center of the left and right cameras at the nth position. is the imaging point of P on the left and right camera imaging planes in the initial posture; is the imaging point of P on the left and right camera imaging planes in the nth posture.

In dynamic binocular vision, the key is to obtain dynamic baseline and depth information. The calculation process includes:

(1) Calculate the mapping relationship from the initial pose R-Camera to R-Base ^rb T _rc(0)

According to equations (3.15) and (3.20), the transformation relationship between P _rc and P _rb in the initial posture can be obtained, as shown in equation (3.29).

Among them, ^rb R _rc(0) and ^rb t _rc(0) are the rotation matrix and translation vector of P _rc relative to P _rb in the initial pose, ^rb R _re(0) and ^rb t _re(0) are the rotation matrix and translation vector of P _re relative to P _rb in the initial pose, ^re(0) R _rc(0) and ^re(0) t _rc(0) are the rotation matrix and translation vector of P _rc relative to P _re in the initial pose.

Then the mapping relationship ^rb T _rc(0) from R-Camera to R-Base in the initial posture can be expressed by formula (3.30).

(2) Calculate the mapping relationship ^rc(n) T _rb from R-Base to R-Camera in the nth pose

Similar to the calculation of ^rb T _rc(0) , the transformation relationship of P _rb relative to P _rc in the nth posture can be obtained, as shown in equation (3.31). Among them, ^rc(n) R _rb and ^rc(n) t _rb are the rotation matrix and translation vector of P _rb relative to P _rc in the nth posture.

Then the mapping relationship ^rc(1) T _rb from R-Base to R-Camera in the nth posture can be expressed by formula (3.32).

(3) Calculate the mapping relationship from World to R-Base ^rb T _w

According to the traditional binocular vision calibration method, the conversion relationship between _Pw and _Prc in the initial posture is obtained as shown in formula (3.33). and are the rotation matrix and translation vector of the right camera at the initial pose relative to the calibration object obtained after camera calibration _[59] .

Combining equations (3.29) and (3.33) we can get the transformation relationship of P _w relative to P _rb , as shown in equation (3.34). Where ^rb R _w and ^rb t _w are the rotation matrix and translation vector of P _w relative to P _rb .

Then the mapping relationship ^rb T _w from World to R-Base can be expressed by formula (3.35).

(4) Calculate the mapping relationship ^rc(n) T _w from World to R-Camera at the nth pose

Combining equations (3.31) and (3.34) we can get the transformation relationship of P _w relative to P _rc in the nth posture, as shown in equation (3.36). Where ^rc(n) R _w and ^rc(n) t _w are the rotation matrix and translation vector of P _w relative to P _rc in the nth posture.

Then the mapping relationship ^rc(n) T _w from World to R-Camera in the nth pose can be expressed by formula (3.37).

(5) Calculate the mapping relationship ^lc(n) T _w from World to L-Camera at the nth pose

Similar to the calculation of ^rc(n) T _w , the transformation relationship of P _w relative to P _lc in the nth posture can be obtained, as shown in formula (3.38). Among them, ^lc(n) R _w and ^lc(n) t _w are the rotation matrix and translation vector of P _w relative to P _lc in the nth posture, ^lc(n) R _lb and ^lc(n) t _lb are the rotation matrix and translation vector of P _lb relative to P _lc in the nth posture, ^lb R _w and ^lb t _w are the rotation matrix and translation vector of P _w relative to P _lb.

Then the mapping relationship ^lc(n) T _w from World to L-Camera in the nth pose can be expressed by formula (3.39).

(6) Calculate the mapping relationship from L-Camera to R-Camera in the nth pose ^rc(n) T _lc(n)

The combination of equations (3.36) and (3.38) can obtain the transformation relationship of P _lc relative to P _rc in the nth posture, as shown in equation (3.40). Among them, ^rc(n) R _lc(n) and ^rc(n) t _lc(n) are the rotation matrix and translation vector of P _lc relative to P _rc in the nth posture.

Then the mapping relationship ^rc(n) T _lc(n) from L-Camera to R-Camera in the nth pose is the relative pose relationship between the left and right cameras in the nth pose. The expression of ^rc(n) T _lc(n) is shown in formula (3.41).

(7) Calculate dynamic baseline and depth information

In formula (3.41), ^rc(n) t _lc(n) is the relative translation vector of the left and right cameras, then the dynamic baseline b at the nth posture can be obtained modulo ^rc(n) t _lc(n) , as shown in formula (3.42).

As shown in Figure 6, the depth information is obtained based on the triangulation principle: In the figure, I _l is the image plane of the left camera, O _l is the optical center of the left camera, its projection on I _l is marked as O _l ', and the projection of the target point P on I _l is marked as P _l (u _l ,v _l ); I _r is the image plane of the right camera, O _r is the optical center of the right camera, its projection on I _r is marked as O _r ', and the projection of P on I _r is marked as P _r (u _r ,v _r ); The distance between P _l and P _r is the difference between the baseline b and the parallax d, as shown in (3.43). Because the specifications of the left and right cameras are consistent, the focal lengths f of the two cameras are the same. z is the vertical distance from P to the baseline b, that is, the depth information.

|P _l P _r |=bd=b-(u _l -u _r )

(3.43)

Depth information can be obtained through the triangle phase principle, as shown in formula (3.44), where b is the dynamic baseline.

Finally, z is combined with ( _ul , _vl ), and the camera internal parameters and projection relationship are used to convert it into the coordinates ( _Xc , _Yc , _Zc ) in the camera coordinate system. Then, according to the coordinate system mapping relationship, ( _Xc , _Yc , _Zc ) is converted into the coordinates (X, Y, Z) in the world coordinate system, that is, the three-dimensional coordinates of the spatial point. According to the above calculation process, the dynamic binocular vision calibration method can be obtained. This method only performs binocular vision calibration before the two cameras move. During the movement, the spatial three-dimensional coordinates of the target object can be obtained only through the coordinate system mapping relationship. There is no need to re-calibrate the binocular vision, which can effectively reduce the influence of external environmental factors on the calibration results.

According to the above embodiments, the improved YOLO target detection algorithm proposed in the present invention is formed by introducing the attention mechanism into the YOLO-v5s network structure. There are three reasons: first, the attention mechanism makes the network more focused on the target object, which helps to improve the detection accuracy and recall rate of YOLOv5s; second, the attention mechanism helps to extract important features in the image, suppress noise and irrelevant information, and help enhance the robustness of YOLOv5s in complex scenes (occlusion, illumination changes, etc.); third, the attention mechanism can guide the YOLOv5s network to learn image features more effectively, which helps to accelerate the convergence speed of the model and reduce resource consumption.

The attention mechanism is a technology used to enhance the expressiveness and accuracy of deep learning models. It can help the model selectively focus on features related to the current task in the input data, so that the model can better capture and utilize key information. Common attention mechanisms include channel attention SE (Squeeze-and-Excitation), CA (ChannelAttention Module), ECA (Efficient Channel Attention Module), spatial attention SA (Spatial Attention) and hybrid attention CBAM (Convolutional Block Attention Module). In the selection of attention mechanisms, because channel attention loses part of the spatial information when embedding the channel, spatial attention is difficult to capture global dependencies, so a hybrid attention mechanism combining channel attention and spatial attention is selected. CBAM loses some local information by learning the information of feature maps through global pooling, and pays insufficient attention to small-scale objects. Therefore, the present invention combines channel attention ECA and spatial attention SA to form a new hybrid attention mechanism, which is called ECA_SA module in this embodiment. Compared with CBAM, ECA_SA pays more attention to adaptive weighting in the channel and spatial dimensions, and adopts separable convolution operations, which performs better when processing small-scale objects.

The choice of the position where the attention mechanism is added is also crucial for the optimization of the model. The Neck network is located between the backbone network and the output layer. It plays a connecting role in the YOLO-v5s model. It is responsible for further processing, fusing and aggregating the features extracted by the backbone network. In the Neck network, adding the attention mechanism before the convolution operation helps the network to better capture the correlation between features. Therefore, the present invention chooses to add an ECA_SA module before each convolution block in the Neck network. The new network model is called YOLO-v5s_ECA_SA, and its network structure is shown in Figure 7. When adding the ECA_SA module, it is necessary to ensure that the number of channels of the ECA_SA module is consistent with that of the two modules before and after it, and at the same time, the connectivity of the ECA_SA module with other modules in the network model should be checked.

According to the above embodiment, preferably, it also includes: using a deconvolution method to replace the original nearest neighbor interpolation method of the YOLO model, which back-propagates the gradient by learning parameters to map the low-resolution feature map back to the high-resolution space. Specifically:

The nearest neighbor interpolation method in the upsample module (Upsample) of the YOLOv5s_ECA_SA network model is replaced by the deconvolution method. The relevant parameters that need to be configured for deconvolution are shown in Table 1.

Table 1 Deconvolution parameter configuration

According to the above embodiments, preferably, the basic idea of the position-based visual servo control method (PBVS) is to link the posture error of the end effector of the robot with the error between the visual feature points or feature descriptors, and minimize these errors by adjusting the joint angles of the robot. The basic process is shown in Figure 8. It can be seen from the basic process of PBVS that two key parameters are required to make the end effector of the robot accurately reach the expected posture: the speed and position in the joint space. According to the dynamic binocular vision calibration method, the posture of the target object can be obtained, so that the PBVS system can be constructed. After the system is built, it is necessary to obtain the movement speed of each joint to control the joint movement for grasping.

The position of the joint space can be continuously adjusted according to the visual servo error. The error of PBVS can be expressed by formula (5.33), where s is the state quantity obtained by visual measurement at time t, l(t) is the three-dimensional space position information at time t, r is the three-dimensional space posture information, and s^ is the ideal state quantity.

e(t)＝s(l(t),r)-s ^{^} (5.33)

Let r = [R _x , R _y , R _z ] ^T , where its modulus is the size of the rotation angle θ, then θ and r can be expressed by equations (5.34) and (5.35).

In an ideal state, s^＝[0,0] ^T , let the rotation matrix and translation vector of the target object coordinates and the robot end coordinates be

^t R _e and ^t T _e , then equation (5.33) can be rewritten as equation (5.36). ^t R _e and ^t T _e can be obtained by dynamic binocular vision calibration.

e(t)＝(tT _e ,r) ^T

(5.36)

Assume that the linear velocity vector and angular velocity vector of the end of the robot arm are _ve and _we respectively, then the velocity of the end is _Ve = ( _ve , _we ), and its conversion relationship with the state quantity is shown in formula (5.37):

s＝L _s V _e (5.37)

Among them, Ls is the characteristic Jacobian matrix, and its expression is shown in formula (5.38):

Let _Le = _Ls . According to equations (5.36) and (5.37), we can get the derivative of the error, as shown in equation (5.39).

e.＝L _e V _e (5.39)

Assuming that the error decreases exponentially, the speed of the gripper is as shown in equation (5.40), where λ is the proportionality coefficient, is a full rank matrix of _Le .

According to equations (5.36) and (5.38), equation (5.40) can be rewritten as shown in equation (5.41).

According to formula (5.41), the linear velocity v _e and angular velocity w _e of the end of the robot arm can be obtained, as shown in formula (5.42).

The posture error e(t), linear velocity v _e and angular velocity w _e are sent to the robot controller, and the motion angle and speed of each joint are calculated based on kinematic analysis, so that the robot can be controlled to accurately grasp the target object.

According to the above embodiment, although PBVS can accurately control the grasping posture of the robot arm, it is sensitive to changes in illumination and viewing angle, and has high requirements for the accuracy of feature point matching. It is difficult to achieve accurate grasping of the robot arm using only PBVS in a dynamic environment. Therefore, a posture estimation algorithm based on the extended Kalman smooth variable structure filter (EK-SVSF) is added to the visual servo control system of the present invention. The Kalman filter provides an optimal estimate of the system state by predicting and updating the state of the system. State prediction of the visual servo system Covariance prediction P _k/k-1 and prediction measurement It can be expressed by formula (5.43),

Where A is the system dynamic matrix, is the a posteriori state estimation vector, P _k-1 is the a posteriori error covariance matrix, Q _k-1 is the system noise covariance matrix, and f is the image mapping function.

According to formula (5.43), the prediction error e _z ,k/k-1 can be obtained, as shown in formula (5.44), where ξ _k-1 is the camera measurement value.

Thus, the smooth boundary value ψ _k is obtained, as shown in formula (5.45), where F _k is the observation matrix, S _k is the measurement error covariance matrix, E- is the diagonal matrix composed of E, and its expression is shown in formula (5.46), where γ is the convergence rate of SVSF.

E＝|e _z ,k/k-1|+γ|e _z ,k-1/k-1| (5.46)

Assuming that the constant boundary value is ψ _c , when ψ _k > ψ _c , the gain K _k of EK-SVSF is a function of the first verification measurement error e _z ,k/k-1 and the post-verification measurement error e _z ,k-1/k-1, that is, the standard SVSF gain; when ψ _k < ψ _c , the gain K _k of EK-SVSF is the standard EKF gain. The expression of the gain of EK-SVSF is shown in (5.47), is the pseudo-inverse of the measurement matrix F, diag represents the diagonal, represents the Hadamard product, and sat represents the logical satisfies condition of e _z ,k/k-1/ψ _k in SVSF.

The updated state prediction ω^ _k/k and covariance prediction P _k of the system are calculated based on K _k , as shown in formula (5.48).

According to the above embodiments, the pose estimation of a single camera will be affected by the external environment, thereby affecting the accuracy of the pose estimation, which is not conducive to grasping tasks in dynamic environments. In order to improve the robustness and stability of the dual-arm system, data fusion technology is usually used to integrate data from multiple sensors in a specific way. The sensor of the present invention includes two left and right cameras. When the main arm performs a grasping task, the main eye will collect target information and perform pose estimation, and the slave arm will reach the side of the main arm to collect target information and main arm end effector information and perform pose estimation. Therefore, the present invention will perform data fusion on the pose estimation to obtain the optimal pose estimation.

In order to obtain better pose estimation, the present invention uses the ordered weighted average operator (OWA) to fuse the EK-SVSF pose estimation results from two cameras. The OWA operator performs weighted averaging based on the order relationship between nodes to obtain the aggregated result. The distributed data fusion process based on OWA is shown in Figure 9.

OWA performs distributed data fusion on the pose estimation results of EK-SVSF. Therefore, the weight of OWA is determined by the covariance matrix P _k/k calculated based on the posterior error e _k/k. The calculation expression is shown in formula (5.49), where e _k/k is the current state ω _k and the state prediction Each time an error estimation is performed, the error value can be obtained from the diagonal elements of the covariance matrix P _k/k and used to update the weights.

According to the principle of the distributed data fusion method based on OWA, the lower the estimated error, the higher the weight, and vice versa. Therefore, the weight is the inverse of the error value. Assuming that the first elements of the two covariance matrices P _k/k are P ₁ and P ₂ , the weights are as shown in formula (5.50). The obtained weights are applied to the corresponding data and weighted averaged to obtain the optimal pose estimation.

According to the above embodiments, the present invention aims to guide the dual-arm collaborative robot with a visual servo system, takes a dynamic environment as the background, and is based on two position-variable EIH cameras. It optimizes the visual servo control method of the dual-arm collaborative robot around key technologies such as binocular vision calibration, target detection algorithm, visual servo control, and pose estimation algorithm, thereby improving the robustness and accuracy of the visual servo control system of the dual-arm collaborative robot. Specifically, it includes: in order to solve the problem that the binocular system is easily blocked in a dynamic environment and causes failure, a dual-arm collaborative robot visual servo control scheme is designed, and a dynamic binocular visual servo system is proposed, which guides the movement of the dual-arm collaborative robot through two position-variable EIH cameras. The main body of the hardware system consists of two collaborative robot arms and two depth cameras, and the visual processing and the movement of the collaborative robot arms are controlled by the ROS system. A dynamic binocular vision calibration method with stronger adaptability to environmental changes is proposed. This method uses the visual module on the collaborative robot to build a dynamic binocular vision system, and realizes calibration through the traditional binocular vision calibration method and coordinate system mapping relationship. The YOLOv5s network model is selected as the target detection algorithm, and improvements are made to the shortcomings of the YOLOv5s model in detecting targets in dynamic environments, including the introduction of the ECA_SA module in the Neck network and the replacement of the nearest neighbor interpolation method in upsampling with the deconvolution method. The position-based visual servo control technology is used, combined with the EK-SVSF pose estimation algorithm and the OWA-based step-by-step data fusion algorithm to estimate the optimal pose of the target.

As shown in FIG10 , according to another embodiment of the present invention, a dual-arm collaborative robot visual servo control device 100 is disclosed, comprising: a memory 101 for storing program instructions; a processor 102 for calling the program instructions stored in the memory to implement the dual-arm collaborative robot visual servo control method as in the above embodiment.

According to another embodiment of the present invention, a computer-readable storage medium is disclosed. The computer-readable storage medium stores program codes, and the program codes are used to implement the dual-arm collaborative robot visual servo control method as described in the above embodiment.

All or part of the steps in the various methods of the above embodiments can be completed by controlling the relevant hardware through a program, and the program can be stored in a readable storage medium, and the storage medium includes a read-only memory (ROM), a random access memory (RAM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), a one-time programmable read-only memory (OTPROM), an electronically erasable programmable read-only memory (EEPROM), a compact disc read-only memory (CD-ROM) or other optical disc storage, magnetic disk storage, magnetic tape storage, or any other readable medium that can be used to carry or store data.

The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims (10)

1. The visual servo control method of the double-arm cooperative robot is characterized by comprising the following steps of:

acquiring image information of a target object acquired by a first camera arranged at the tail end of a first mechanical arm and a second camera arranged at the tail end of a second mechanical arm;

Establishing a coordinate relationship between the mechanical arm and the target object based on camera calibration and hand-eye calibration;

Performing static binocular vision calibration once only in the initial pose, and then obtaining the space three-dimensional coordinates of the target object through the coordinate system mapping relation, so as to realize dynamic binocular vision;

identifying the target object based on an improved YOLO target detection algorithm formed by adding an ECA attention mechanism and an SA attention mechanism on the basis of a YOLO-v5s model;

Determining target object position information according to a target detection result;

determining a master-slave relationship of the first mechanical arm and the second mechanical arm according to the position information;

The master arm is controlled to reach the grabbing pose, and the slave arm is controlled to drive the camera to reach the observing position according to the pose of the master arm;

Adopting a visual servo control method based on position, and carrying out optimal pose estimation on a target object by combining an EK-SVSF pose estimation algorithm and an OWA-based step-by-step data fusion algorithm;

and controlling the main arm to carry out deviation adjustment through joint movement according to the result of the optimal pose estimation, so as to realize accurate grabbing of the target object.

2. The method according to claim 1, wherein the first camera and the second camera are depth cameras, respectively, following the free end movements of the first mechanical arm and the second mechanical arm.

3. The visual servo control method of the double-arm cooperative robot according to claim 1, wherein the calculation process of the camera calibration comprises:

fixing a camera and continuously collecting images of a calibration plate;

Calculating the pixel coordinates of the corner points;

Calculating a homography matrix;

solving an internal reference matrix;

solving an external parameter matrix;

Solving a lens distortion coefficient;

and (3) based on the minimum function of the reprojection error, iterating by using maximum likelihood estimation, and optimizing all parameters obtained by camera calibration.

4. The visual servo control method of the double-arm cooperative robot according to claim 1, wherein the calculation process of the hand-eye calibration comprises:

Calculating a mapping matrix from a mechanical arm terminal coordinate system to a base coordinate system;

calculating a mapping matrix from the world coordinate system to the camera coordinate system;

and calculating a mapping matrix from the camera coordinate system to the tail end coordinate system of the mechanical arm.

5. The method for controlling visual servoing of a dual-arm cooperative robot according to claim 1, wherein said calculating of dynamic binocular vision comprises:

Calculating a mapping relation from the first camera to the base coordinate system under the initial pose;

Calculating the mapping relation from the base coordinate system to the first camera under the nth pose;

calculating the mapping relation from the world coordinate system to the base coordinate system;

calculating the mapping relation between the world coordinate system under the nth pose and the first camera;

calculating the mapping relation from the world coordinate system to the second camera under the nth pose;

Calculating a mapping relation from the second camera to the first camera under the nth pose;

calculating a dynamic baseline in the nth pose according to the relative translation vector of the first camera and the second camera;

obtaining depth information according to the dynamic baseline and a triangulation principle;

And acquiring the three-dimensional coordinates of the space point by utilizing the internal camera parameters and the projection relation according to the depth information and the projection information of the target point on the second camera image plane.

6. The visual servo control method of the double-arm cooperative robot according to claim 1, wherein the improved YOLO target detection algorithm specifically comprises:

The ECA module and SA module are added before each convolution block of Neck layers of the YOLO-v5s model.

7. The method for visual servoing control of a dual-arm cooperative robot of claim 6, further comprising:

The nearest neighbor interpolation method of an up-sampling module of the YOLO-v5s model is replaced by a deconvolution method, the convolution kernel size is 4, the convolution step length is 2, the gradient is reversely propagated through learning parameters, and the low-resolution feature map is mapped back to a high-resolution space.

8. The method according to any one of claims 1 to 6, wherein the step of determining the master-slave relationship between the first and second robot arms specifically comprises:

Determining a center point of the target object through a vision system;

Calculating the Euclidean distance Ll between the center point of the object and the first mechanical arm, and calculating the Euclidean distance Lr between the center point of the object and the second mechanical arm;

When Ll is smaller than Lr, selecting the first mechanical arm as a master arm and the second mechanical arm as a slave arm;

when Ll is larger than Lr, the second mechanical arm is selected as a master arm, and the first mechanical arm is selected as a slave arm.

9. A visual servo control device of a double-arm cooperative robot, comprising:

a memory for storing program instructions;

A processor for invoking said program instructions stored in said memory to implement the dual arm cooperative robot vision servo control method of any of claims 1-8.

10. A computer readable storage medium, characterized in that the computer readable storage medium stores a program code for implementing the double-arm cooperative robot vision servo control method according to any one of claims 1 to 8.