CN112894815B - Optimal pose detection method for object grasping by visual servo robotic arm - Google Patents

Abstract

The invention discloses a method for detecting the optimal grabbing pose of a visual servo mechanical arm, which mainly comprises the following steps: step one, reading a photo; step two, extracting SURF characteristic points; thirdly, generating a feature vector of the image according to the SURF feature points; step four, a matching pair (containing a wild value) is initially established; step five, preventing affine change and removing outliers which do not meet the change; step six, acquiring a polygonal frame of the target object; resolving an optimal pose under a two-dimensional coordinate system; and step eight, controlling the mechanical arm to grab the article by the servo control. The method combines monocular vision with the servo mechanical arm, and further obtains the optimal pose angle of the target object by utilizing the geometric relation of slope obtained between two points, namely the angle of rotation grasped by the servo mechanical arm, so that the method is simple and easy to implement, has few occupation problems, is high in calculation speed, and has wide application prospect.

Description

Optimal pose detection method for object grasping by visual servo robotic arm

technical field

The invention relates to the technical field of visual processing, in particular to a method for detecting the optimal posture and attitude of a visual servo robotic arm for object grasping.

Background technique

Picking up mobile phones in narrow locations such as sewers and seams is a widespread problem. Using the mechanical arm structure to grab items is a more effective solution. However, there has been a lack of item positioning and action commands that match the mechanical arm structure for such problems. method. Before the robot automatically grasps the target object, it should first determine the pose of the object to be grasped, that is, to determine the appropriate grasping position and grasping posture, and machine vision detection is the most commonly used target posture detection method; Visual pose extraction methods mainly include two categories: monocular vision and binocular vision. Monocular vision is easy to operate and fast to process data information, but it lacks depth information, which makes it difficult to measure depth; binocular vision uses the same point in space to match imaging points on two cameras, which can measure depth, and can perform simple The disadvantage of positioning and attitude determination of objects with simple shapes in the environment is that the calculation process involves a large number of image matching and target recognition algorithms, which can only be used for targets with simple geometric relationships, and cannot be applied in complex backgrounds. The simple shape of such objects picked up by the mobile phone in a narrow position is just suitable for monocular vision.

Therefore, it is necessary to invent a method based on monocular vision, which has the advantages of fast calculation speed and low resource consumption.

SUMMARY OF THE INVENTION

Purpose of the invention: In order to overcome the deficiencies in the prior art, the present invention provides a method for detecting the optimal position and attitude of a visual servo robotic arm for object grasping based on monocular vision, with fast calculation speed and low resource occupation.

Technical solution: In order to achieve the above purpose, the visual servo robot arm grabbing the best posture detection method of the present invention is characterized in that: it includes the following steps:

Step 1, read the photo;

Step 2, extract SURF feature points;

Step 3, generate the feature vector of the image according to the SURF feature points;

Step 4, initially establish a matching pair (including outliers);

Step 5: Prevent affine changes and remove outliers that do not satisfy the changes;

Step 6, obtaining the polygon frame of the target object;

Step 7: Calculation of the optimal pose in the two-dimensional coordinate system;

Step 8, the servo-controlled robotic arm grabs the item.

Further, in step 1, the camera imaging model is used for photo shooting and camera calibration; the camera imaging model is composed of the world coordinate system _Ow - _XwYwZw , the _{camera coordinate system Oc - XcYcZc} , the pixel coordinate system Op-uv and the image coordinate system O _i -X _i Y _i ; where _P is a point in the camera coordinate system, and the coordinates are (X _c , Y _c , Z _c ); P' is in the image The imaging point of the image coordinate system is (x, y) in the image coordinate system and (u, v) in the pixel coordinate system; camera calibration is to determine the camera coordinate system, image coordinate system, pixel coordinate system and the real coordinate system. Relationship.

Further, in step 2, include the following sub-steps,

Sub-step 1, establish an integral image; calculate the sum of pixels in any area by simple addition and subtraction, and the calculation formula of the sum of gray values of all pixels in the rectangle ABCD is:

∑=A-B-C+D

Sub-step 2, establish a Hessian matrix; a pixel point (x, y), let the Hessian matrix of the function f(x, y) be defined as:

It can be seen that the H matrix is composed of the second-order second-order partial derivative of the function f, and each pixel point can solve an H-matrix; in the formula: L _xx (x, σ) is the image f and the Gaussian with scale σ. The convolution of the first derivative is defined as follows:

The definitions are similar for L _xy (x,σ) and L _yy (x,σ).

Sub-step 3, establish the scale space; the detection algorithm and extraction of SUFR feature extreme points are based on the scale space theory; by changing the scale of the Gaussian filter, not changing the image itself, the response to different scales is formed.

Sub-step 4, extract feature points; use non-extreme values to remove certain feature points, compare the size of each pixel point of the Hessian determinant image with the 26 points in the 3-dimensional neighborhood, set a threshold, and use 3-dimensional After the linear interpolation method obtains the sub-pixel-level feature points, the points whose feature values are less than the threshold are removed to obtain more stable points.

Sub-step 5, select the direction of the feature point; take the feature point as the center, detect the point of interest to determine the scale s, count the Haar wavelet responses of all points within the radius sector, and give greater weight to the pixels close to the feature point; count the area The sum of all Harr wavelet responses forms a vector, that is, the direction of the fan; traverse all the fan regions, and select the longest vector direction as the direction of the feature point.

Sub-step six, construct the feature point descriptor of SURF; take the feature point as the center, construct a square window with 20s as the side length, s is the scale where the feature point is located; divide it into 16 4×4 sub-regions, The sum of the Haar wavelet features of the horizontal x and vertical y of 25 pixel elements in each sub-region is ∑H _x and ∑H _y respectively; the 4-dimensional descriptor V=[∑H _x ,∑ H _y ,∑|H _x |,∑|H _y |], the feature descriptor with the feature vector length of 64 dimensions is obtained.

Further, in step seven,

Convert the positional relationship of the image in the pixel coordinate system _Op-uv to the physical coordinates O _i -X _i Y _i of the image, the purpose is to calculate the geometric relationship in the physical coordinate system of the image, and the conversion formula is in the form of a matrix ( 4):

where (u, v) are the pixels in the horizontal and vertical directions, let O _i (u ₀ , v ₀ ) be the pixel at the center of the coordinate system of the image, and dx and dy are the x and y axes of each pixel The actual physical size in the direction can be known from the calibration of the camera u ₀ , v ₀ , dx, dy; in the rectangular frame CDEF of the item matched by the SURF invariant feature point algorithm, the center of mass of the item O ₄ (u 4 (u _s , v _s ) and the pixel coordinates of CDEF, according to the geometric relationship, the pixel coordinates of the grasping points A (u ₁ , v ₁ ) and B (u ₂ , v ₂ ) of the item can be obtained, and the matched item rectangle frame;

Bring the pixel coordinates of the grasping points A(u ₁ , v ₁ ) and B(u ₂ , v ₂ ) of the object into formula (4) to derive the physical coordinates of A and B, such as formula (5):

and formula (6):

The obtained image physical coordinates of A(x ₁ , y ₁ ) and B(x ₂ , y ₂ ) can solve the included angle α, that is, the optimal pose angle. In theory, the rotation angle of the servo to the manipulator should also be α, but the rotation angle is less than or greater than α due to the systematic error of the manipulator, so the rotation angle is taken as β; the clockwise rotation of the manipulator grasping the relative object is positive, and the counterclockwise rotation is negative. The slope of the geometric relationship between can be calculated, and the formula (7) can be obtained:

Formula (8) can be obtained from formula (5) (6) (7):

Find the angle α in formula (8), the value range is [0, 90°], if it is positive, the manipulator will rotate clockwise, and if it is negative, it will rotate counterclockwise. α is the pose angle of the object in the two-dimensional plane and also the gripping angle of the gripping rotation of the manipulator, and its gripping points are two points A and B. It can be seen from formula (8) that dx and dy are internal parameters of the camera, so α is only related to the difference between the pixel points of the clamping points A and B, which simplifies the complex calculation in the image, thereby improving the visual servo effect.

Beneficial effects: The visual servo robotic arm grasps the best pose detection method of the present invention, which combines monocular vision and servo robotic arms, and realizes SURF invariant features through processes such as camera calibration, image acquisition, and image preprocessing. The point is matched to the target object to complete the target detection. Through the principle of camera imaging, the position transformation relationship from the pixel coordinate system to the image coordinate system is calculated, and the optimal pose angle of the target object is obtained by using the geometric relationship between the two points to find the slope, that is, the angle that the servo manipulator should rotate. The algorithm is simple and easy to implement, can effectively avoid the problem of image matching resource occupancy in the process of binocular vision pose measurement, and the calculation speed is fast. Experiments have verified the effectiveness and accuracy of the algorithm, which can complete the detection of the best pose of the object and the determination of the grasping point of the manipulator, which meets the requirements of the visual servo manipulator for grasping. At the same time, the measurement algorithm can be applied to industrial parts positioning, palletizing robot handling and other fields, and has broad application prospects.

Description of drawings

Figure 1 is an overall schematic diagram of a robotic arm grabbing an item system;

Figure 2 is a schematic diagram of a camera imaging model;

3 is a schematic diagram of a marking process;

Figure 4 is a schematic diagram of the fast calculation of the integral graph;

Figure 5 is a schematic diagram of the pose solution;

6 is a schematic diagram of a rectangular frame of matching items;

Fig. 7 is the summary diagram of MATLAB toolbox calibration experiment;

FIG. 8 is a schematic diagram of the experimental operation of pose detection under a single background;

FIG. 9 is a schematic diagram of the experimental operation of pose detection in a complex background;

Figure 10 is a demonstration of the pose solution and object grasping.

Detailed ways

The present invention will be further described below in conjunction with the accompanying drawings.

There are generally two relationships between the position of the robotic arm and the camera: one is that the camera is eye-in-hand on the robotic arm, which is highly maneuverable and can obtain high image data; the other is that the camera is eye-to-hand outside the robotic arm -hand, its advantages are that the camera has a wide field of view and high flexibility, but the robotic arm is easily affected by the position of the camera. In this paper, the camera is used in the eye-in-hand of the manipulator. After the calibration of the camera and the end effector of the manipulator, a fixed transformation relationship can be obtained. O1O2O3O4 respectively represent the coordinate system of the manipulator, the coordinate system of the manipulator, the coordinate system of the camera, and the coordinate of the item. origin of the system. The principle of the scheme is shown in Figure 1.

The method for detecting the optimal pose and posture for grasping by a visual servo manipulator is characterized by comprising the following steps:

Step 1, read the photo;

Step 2, extract SURF feature points;

Step 3, generate the feature vector of the image according to the SURF feature points;

Step 4, initially establish a matching pair (including outliers);

Step 5: Prevent affine changes and remove outliers that do not satisfy the changes;

Step 6, obtaining the polygon frame of the target object;

Step 7: Calculation of the optimal pose in the two-dimensional coordinate system;

Step 8, the servo-controlled robotic arm grabs the item.

In step 1, the camera imaging model is used for photo shooting and camera calibration; the camera imaging model is composed of the world coordinate system O _w -X _w Y _w Z _w , the camera coordinate system O _c -X _c Y _c Z _c , the pixel coordinates The system O _p -uv and the image coordinate system O _i -X _i Y _i are composed; where P is a point in the camera coordinate system, and the coordinates are (X _c , Y _c , Z _c ); P' is the imaging point in the image , the coordinates in the image coordinate system are (x, y), and the pixel coordinate system is (u, v); the imaging principle model is shown in Figure 2; the camera calibration is to determine the camera coordinate system, image coordinate system, pixel coordinate system relation to the real coordinate system. Specifically, the mathematical model is created by the imaging principle of the camera, and the model parameters of the camera are deduced with the help of the pixel coordinates of the known features and the corresponding world coordinates. The internal and external parameters of the camera are calibrated as shown in Figure 3.

In step 2, the following sub-steps are included,

Sub-step 1, establish an integral image; calculate the sum of the pixels in any area by simple addition and subtraction, as shown in Figure 4, the calculation formula (1) of the sum of the gray values of all pixels in the rectangle ABCD is:

∑=A-B-C+D

Sub-step 2, establish a Hessian matrix; a pixel point (x, y), set the Hessian matrix definition (2) of the function f(x, y) as:

It can be seen that the H matrix is composed of the second-order second-order partial derivative of the function f, and each pixel point can solve an H-matrix; in the formula: L _xx (x, σ) is the image f and the Gaussian with scale σ. The definition of the convolution of the order derivative (3) is as follows:

The definitions are similar for L _xy (x,σ) and L _yy (x,σ).

Sub-step 3, establish the scale space; the detection algorithm and extraction of SUFR feature extreme points are based on the scale space theory; by changing the scale of the Gaussian filter, not changing the image itself, the response to different scales is formed.

Sub-step 4, extract feature points; use non-extreme values to remove certain feature points, compare the size of each pixel point of the Hessian determinant image with the 26 points in the 3-dimensional neighborhood, set a threshold, and use 3-dimensional After the linear interpolation method obtains the sub-pixel-level feature points, the points whose feature values are less than the threshold are removed to obtain more stable points.

Sub-step 5, select the direction of the feature point; take the feature point as the center, detect the point of interest to determine the scale s, count the Haar wavelet responses of all points within the radius sector, and give greater weight to the pixels close to the feature point; count the area The sum of all Harr wavelet responses forms a vector, that is, the direction of the fan; traverse all the fan regions, and select the longest vector direction as the direction of the feature point.

Sub-step six, construct the feature point descriptor of SURF; take the feature point as the center, construct a square window with 20s as the side length, s is the scale where the feature point is located; divide it into 16 4×4 sub-regions, The sum of the Haar wavelet features of the horizontal x and vertical y of 25 pixel elements in each sub-region is ∑H _x and ∑H _y respectively; the 4-dimensional descriptor V=[∑H _x ,∑ H _y ,∑|H _x |,∑|H _y |], the feature descriptor with the feature vector length of 64 dimensions is obtained.

In step seven,

The optimal pose detection of items includes object detection with SURF invariant feature point matching and its pose solution. After calibrating the camera on the robotic arm, solve the internal and external parameter matrix of the camera, and perform the best pose detection and calculation of the image of the object detected by matching the SURF invariant feature points in a two-dimensional coordinate system. The principle process of the solution is shown in Figure 5. (a) (b).

The principle process is as follows: In the image coordinate system Oi-XiYi, set the barycentric coordinate of the object as O4(sx,sy), set the deviation distance between the grip center O2(zx,zy) at the end of the manipulator and the optical axis center Oi of the camera e, the gesture of the finger grasped by the calibration robot is PQ, and the projection relationship with the imaging of the camera is shown in Figure 5(a), where Op-u v is the pixel coordinate system.

The servo manipulator moves from (a) to (b) state so that the center O2 of its grasp coincides with the center of mass O4 of the object, as shown in Figure 5(b). The angle between the y-axis of the object's attitude and the Xi-axis of the image coordinate system Oi-XiYi is α. The object does not move and drives the manipulator to grasp and rotate by an angle of α, so that the finger PQ coincides with the y-axis of the object's attitude in projection, that is, the manipulator grasps and rotates α. The new posture of the finger PQ after the angle is P1Q1, and the grasping points are A(x1, y1) and B(x2, y2). The rotation angle α and the new pose P1Q1 of the fingers provide the accuracy for the servo robotic arm to grasp items.

Convert the positional relationship of the image in the pixel coordinate system O _{pu v} to the physical coordinates O _i -X _i Y _i of the image, the purpose is to calculate the geometric relationship in the physical coordinate system of the image, and the conversion formula is in matrix form (4 ):

where (u, v) are the pixels in the horizontal and vertical directions, let O _i (u ₀ , v ₀ ) be the pixel at the center of the coordinate system of the image, and dx and dy are the x and y axes of each pixel The actual physical size in the direction can be known from the calibration of the camera, u ₀ , v ₀ , dx, dy; in the rectangular frame CDEF of the item matched by the SURF invariant feature point algorithm, the center of mass O ₄ (u 4 (u _s , v _s ) and the pixel coordinates of CDEF, according to the geometric relationship, the pixel coordinates of the grasping points A (u ₁ , v ₁ ) and B (u ₂ , v ₂ ) of the item can be obtained, and the matched item rectangle frame;

Bring the pixel coordinates of the grasping points A(u ₁ , v ₁ ) and B(u ₂ , v ₂ ) of the object into formula (4) to derive the physical coordinates of A and B, such as formula (5):

and formula (6):

The obtained image physical coordinates of A(x ₁ , y ₁ ) and B(x ₂ , y ₂ ) can solve the included angle α, that is, the optimal pose angle. In theory, the rotation angle of the servo to the manipulator should also be α, but the rotation angle is less than or greater than α due to the systematic error of the manipulator, so the rotation angle is taken as β; the clockwise rotation of the manipulator grasping the relative object is positive, and the counterclockwise rotation is negative. The slope of the geometric relationship between can be calculated, and the formula (7) can be obtained:

Formula (8) can be obtained from formula (5) (6) (7):

Find the angle α in formula (8), the value range is [0, 90°], if it is positive, the manipulator will rotate clockwise, and if it is negative, it will rotate counterclockwise. α is the pose angle of the object in the two-dimensional plane and also the gripping angle of the gripping rotation of the manipulator, and its gripping points are two points A and B. It can be seen from formula (8) that dx and dy are internal parameters of the camera, so α is only related to the difference between the pixel points of the clamping points A and B, which simplifies the complex calculation in the image, thereby improving the visual servo effect.

From the design principle and solution results of the algorithm, compared with the traditional pose detection algorithm, there is a great improvement in calculation. For example, the typical PNP problem was first proposed by Fischler et al. in 1981, which is to solve the pose problem of the target relative to the camera according to the image coordinates of the target point and the camera imaging model, and in the process of solving the plane PNP problem, nonlinearity will be used. Although the algorithm has high accuracy, it generally requires iteration, requires a large amount of calculation, and has stability problems. In the process of further research in the later stage, typical algorithms include the POSIT algorithm proposed by DENIS OBERKAMPF; the plane target pose estimation algorithm (SP) proposed by Gerald Schweighofer; the EPnP algorithm proposed by Lepetit, and the orthogonal iterative algorithm proposed by Lu. These typical algorithms are nonlinear in the process of solving pose and pose, so they will occupy the resource problem in the process of image solving. The proposed algorithm can avoid complex iterative problems and reduce the problem of resource occupation.

MATLAB 2018b version software and MATLAB hardware support package usbwebcams were selected for camera calibration and target detection experiments, and the operating system was 64-bit Win10 system. The right arm of the Rethink dual-arm robot is selected for the pose calculation and grasping experiments of the servo manipulator, and its operating system is Ubuntu16.04, a 64-bit Linux system, for grasping verification.

The following four points need to be completed before the right arm of the Rethink dual-arm robot performs the pose detection and object grasping experiments: first: calibration of the internal and external parameters of the camera on the right arm; second: the position correction between the grip center and the camera center; The third: the UI display interface window size of the dual-arm robot is calibrated to 640×480 pixels; the fourth: the initial position of the end of the right arm is calibrated so that it can move and search along the horizontal plane.

Camera calibration experiment

The camera on the robotic arm has a maximum resolution of 1280×800 pixels, the calibration board is a 10×7 checkerboard, and the side length of each grid is 28mm×28mm. It collects 20 pictures with a pixel size of 480×640 and imports them into the MATLAB tool Camera calibration in the box.

In the calibration experiment of the MATLAB toolbox, the camera calibration results need to be evaluated, which can be analyzed through the projection error analysis of the camera and the visualization of the three-dimensional external parameters centered on the camera and the calibration plate. The specific calibration process is shown in Figure 7.

After calibrating the experiment in the MATLAB toolbox, enter cameraParams.IntrinsicMatrix in its command line to obtain the camera's internal parameter matrix, and enter RadialDistortion and TangentialDistortion to obtain the radial distortion and tangential distortion coefficients of the camera. The specific physical parameters of the camera are as follows shown in Table 1.

SURF feature point target detection and pose calculation experiment

In order to verify the effectiveness of the proposed algorithm for grasping objects by the servo manipulator, the pose detection experiment was completed, which included the target detection experiment of SURF invariant feature points and the pose solution experiment. At the same time, in order to make the experiment more convincing, the author completed two sets of pose detection comparison experiments under a single background and a complex background (without occlusion) to observe whether the complex background will affect the algorithm experiment effect of pose detection. The experimental results of the pose detection experiments under five groups of single backgrounds and five groups of complex backgrounds are respectively taken for analysis, comparison and evaluation. The analysis table of the experimental results is shown in Table 2. The experimental process of pose detection in a single background and the experimental process of pose detection in a complex background are shown in Figures 8 and 9.

Table 1 Camera calibration results

Experiments show that the number of feature points extracted from SURF invariant feature points in a single background is relatively large, but the intersection points are prone to occur; although the number of extracted feature points in complex backgrounds is small, there are also fewer intersection points. The correct rate and matching time of feature point matching are comparable. The overall matching effect of a single background is slightly better than that of a complex background, but from the overall experimental results, the algorithm can meet the requirements of detecting objects in a complex background. Whether in a single background or in a complex background, the feature point matching speed and effect of the SURF invariant feature point matching experiment are relatively accurate and stable, and can be matched to any deflection angle of the item on the o-xy plane, that is, the item For the pose on the two-dimensional plane, the two theoretical gripping points A and B of the robotic finger can be solved by using the image algorithm and the proposed pose calculation algorithm, and then the pose angle α can be obtained.

The accuracy of the calculated pose angle α is verified through the grasping experiment of the object. The grasping experiment is based on the hand grasping as the reference object, taking six clockwise and anti-clockwise object states, and the pixel size is 640×480 pixels. , is the pose state of the two-dimensional coordinate system in the complex background. From the camera calibration experiment and the SURF feature point matching experiment, the internal parameters u0, v0, dx, dy of the camera and the two gripping points A and B of the manipulator grasping the object can be obtained, and the optimal position can be solved by substituting into formula (8). Attitude angle α, that is, the hand grasping angle β at the end of the manipulator should be rotated. There is a certain error between the calculated pose angle α and the angle β of the grip rotation at the end of the manipulator. In order to verify whether the error will affect the grasping of the item, the error analysis table of the pose angle and the grip rotation angle is listed, as shown in Table 3. shown.

Table 2 Analysis of pose detection results in single background and complex background

In Table 3, there is a certain error between the pose angle α and the rotation angle β output to the manipulator, and it is analyzed that the main source of the error is the rotation accuracy of the steering gear in the grip at the end of the double-arm robot manipulator. The rotation accuracy of the steering gear It can only be accurate to 0.1 bit, which belongs to the absolute error of the system, which is inevitable; after experimental grasping, it is found that the relative error comes from the light, the texture of the surface of the object, etc. During the experimental grasping process, it is found that the error is less than 1 to 2°, which does not affect the grasping of items. In order to avoid the collision of the robotic arm during grasping, the author first simulates the grasping of objects on the PC side, and finally downloads the compiled code to the dual-arm robotic system. The selected 12 groups of object grasping experiments have well verified the effectiveness of the proposed algorithm and provided accuracy for visual grasping. The pose calculation and grasping experiments are shown in Figure 10.

Using the monocular vision system to measure the best pose detection algorithm of the object, the algorithm combines monocular vision and servo manipulator, through the process of camera calibration, image acquisition, image preprocessing, etc., to achieve SURF invariant feature points matching to the target object, complete target detection. Through the principle of camera imaging, the position transformation relationship from the pixel coordinate system to the image coordinate system is calculated, and the optimal pose angle of the target object is obtained by using the geometric relationship between the two points to find the slope, that is, the angle that the servo manipulator should rotate. The algorithm is simple and easy to implement, and can effectively avoid the problem of image matching resource occupancy in the process of binocular vision pose measurement. Experiments have verified the effectiveness and accuracy of the algorithm, which can complete the detection of the best pose of the object and the determination of the grasping point of the manipulator, which meets the requirements of the visual servo manipulator for grasping. At the same time, the measurement algorithm can be applied to industrial parts positioning, palletizing robot handling and other fields, and has broad application prospects.

Table 3 Error analysis table of pose angle and hand rotation angle

The above is only the preferred embodiment of the present invention, it should be pointed out that: for those skilled in the art, without departing from the principle of the present invention, several improvements and modifications can also be made, and these improvements and modifications are also It should be regarded as the protection scope of the present invention.

Claims (1)

1. The method for detecting the best position and posture of an object grasped by a visual servo manipulator is characterized in that: comprising the following steps, Step 1, read the photo; Step 2, extract SURF feature points; Step 3, generate the feature vector of the image according to the SURF feature points; Step 4: Preliminarily establish a matching pair with outliers; Step 5: Prevent affine changes and remove outliers that do not satisfy the changes; Step 6, obtaining the polygon frame of the target object; Step 7: Calculation of the optimal pose in the two-dimensional coordinate system; Step 8: Servo control the robotic arm to grab the item; In step 1, the camera imaging model is used for photo shooting and camera calibration; the camera imaging model is composed of the world coordinate system O _w -X _w Y _w Z _w , the camera coordinate system O _c -X _c Y _c Z _c , the pixel coordinates The system O _p -uv and the image coordinate system O _i -X _i Y _i are composed; where P is a point in the camera coordinate system, and the coordinates are (X _c , Y _c , Z _c ); P' is the imaging point in the image , the coordinates in the image coordinate system are (x, y), and the pixel coordinate system is (u, v); the camera calibration is to determine the relationship between the camera coordinate system, the image coordinate system, the pixel coordinate system and the real coordinate system; In step 2, the following sub-steps are included, Sub-step 1, establish an integral image; calculate the sum of pixels in any area by simple addition and subtraction, and the calculation formula of the sum of gray values of all pixels in the rectangle ABCD is: Σ=A-B-C+D Sub-step 2, establish a Hessian matrix; a pixel point (x, y), let the Hessian matrix of the function f(x, y) be defined as:

It can be seen that the H matrix is composed of the second-order partial derivative of the function f, and each pixel point can solve an H matrix; in the formula: L _xx (x,σ) is the function f and the Gaussian second-order derivative of the scale σ. Convolution is defined as follows:

The definitions are similar for L _xy (x,σ) and L _yy (x,σ); Sub-step 3, establish a scale space; the detection algorithm and extraction of SUFR feature extreme points are based on the scale space theory; by changing the scale of the Gaussian filter, not changing the image itself, the response to different scales is formed; Sub-step 4, extract feature points; use non-extreme values to remove certain feature points, compare the size of each pixel point of the Hessian determinant image with the 26 points in the 3-dimensional neighborhood, set a threshold, and use 3-dimensional After the linear interpolation method obtains the sub-pixel level feature points, the points whose feature values are less than the threshold are removed to obtain more stable points; Sub-step 5, select the direction of the feature point; take the feature point as the center, detect the point of interest to determine the scale s, count the Haar wavelet responses of all points in the sector, and give greater weight to the pixels close to the feature point; count all the points in the area. The sum of the Haar wavelet responses forms a vector, that is, the direction of the fan; traverse all the fan regions, and select the longest vector direction as the direction of the feature point; Sub-step six, construct the feature point descriptor of SURF; take the feature point as the center, construct a square window with 20s as the side length, s is the scale where the feature point is located; divide it into 16 4×4 sub-regions, The sum of the Haar wavelet features of horizontal x and vertical y of 25 pixel elements in each sub-region is ΣH _x and ΣH _y respectively; the 4-dimensional descriptor V=[ΣH _x ,ΣH _y of each sub-region is counted ,∑|H _x |,∑|H _y |], the feature descriptor whose feature vector length is 64 dimensions is obtained; In step seven, Convert the positional relationship of the image in the pixel coordinate system _Op-uv to the physical coordinates O _i -X _i Y _i of the image, the purpose is to calculate the geometric relationship in the physical coordinate system of the image, and the conversion formula is in the form of a matrix ( 4):

where (u, v) are the pixels in the horizontal and vertical directions, let O _i (u ₀ , v ₀ ) be the pixel at the center of the coordinate system of the image, and dx and dy are the x and y axes of each pixel The actual physical size in the direction can be known from the calibration of the camera, u ₀ , v ₀ , dx, dy; in the rectangular frame CDEF of the item matched by the SURF invariant feature point algorithm, the center of mass O ₄ (u 4 (u _s , v _s ) and the pixel coordinates of CDEF, according to the geometric relationship, the pixel coordinates of the grasping points A (u ₁ , v ₁ ) and B (u ₂ , v ₂ ) of the item can be obtained, and the matched item rectangle frame; Bring the pixel coordinates of the grasping points A(u ₁ , v ₁ ) and B(u ₂ , v ₂ ) of the object into formula (4) to derive the physical coordinates of A and B, such as formula (5):

and formula (6):

The obtained image physical coordinates of A(x ₁ , y ₁ ) and B(x ₂ , y ₂ ) can solve the included angle α, that is, the optimal pose angle. In theory, the rotation angle of the servo to the manipulator should also be α, but the rotation angle is less than or greater than α due to the systematic error of the manipulator, so the rotation angle is taken as β; the clockwise rotation of the manipulator grasping the relative object is positive, and the counterclockwise rotation is negative. The slope of the geometric relationship between can be calculated, and the formula (7) can be obtained:

Formula (8) can be obtained from formula (5) (6) (7):

Find the angle α in formula (8), the value range is [0, 90°], if it is positive, the robot rotates clockwise, and if it is negative, it rotates counterclockwise; α is the pose angle of the object in the two-dimensional plane, which is also the robot grasp. The clamping angle of the rotation, the clamping points are A and B; from formula (8), dx and dy are internal parameters of the camera, so α is only the difference between the pixel points of the clamping points A and B. Related, simplifies the complex calculations in the image, thereby improving the visual servo effect.

Images