CN110490934A - Mixing machine vertical blade attitude detecting method based on monocular camera and robot - Google Patents

Abstract

The invention relates to a method for detecting the vertical blade attitude of a hybrid machine based on a monocular camera and a robot. The method first obtains the hand-eye calibration relationship between the robot and the monocular camera, and adjusts the monocular camera to a suitable position by adjusting the robot. After using the monocular camera to collect the image of the mixer blade, image processing and feature extraction are performed on it, and the angle relationship between the vertical mixer blade and the camera can be solved through the imaging principle of the monocular camera and related geometric relationships, and then used The relationship between the calibrated camera and the robot's hand-eye is solved to obtain the rotation angle of the blade in the robot's base coordinate system, that is, the attitude of the blade.

Description

Attitude detection method of vertical propeller of hybrid machine based on monocular camera and robot

technical field

The invention relates to the field of pose detection in high-risk and explosive environments, in particular to a method for detecting the pose of a vertical blade of a hybrid machine based on a monocular camera and a robot.

Background technique

The background of the invention is the production workshop of aerospace explosives, which is a flammable, explosive and high-risk environment. At present, there are many researches on attitude recognition of spacecraft in the field of aerospace, but there are few related literatures on attitude recognition of objects in high-risk workshops. The vertical blades of the mixer are used for rocket propellant mixing. They are located in a flammable and explosive workshop. Before using robots to perform related operations on them, their spatial attitude needs to be detected. Commonly used methods for visually solving the target pose mainly include monocular cameras, binocular cameras, etc. Compared with the monocular camera, the binocular camera stereo calibration correction matching algorithm is complex, the efficiency is relatively low, and the binocular camera system has a large size, which is not suitable for installation in industrial sites with small working space and at the end of industrial robots. Considering the need for explosion-proof treatment of the visual sensor, a monocular camera with a smaller size, easier installation, and a more stable system is selected to detect the attitude of the vertical mixer blade.

Invention patent CN201410016272.0 discloses a dynamic target position and attitude measurement method based on monocular vision at the end of the manipulator. First, camera calibration and hand-eye calibration are performed, and then two images are captured by the monocular camera, and then target feature points are extracted and Perform feature point matching, and then solve the rotation transformation matrix of the camera, perform three-dimensional reconstruction and scale correction on the feature points, and finally use the reconstructed feature points to obtain the position and attitude of the target relative to the camera. This method effectively utilizes the mobile performance of the robotic arm to collect images of the target at two different positions. The principle is similar to the binocular camera system but has greater flexibility and stability. In the feature point matching process, there are many requirements for the number of feature points. For the harsh industrial measurement environment, it is difficult to provide a large number of feature points for the target to be measured. In addition, for the target to be measured with a complex shape, the 3D reconstruction algorithm calculates The amount is large and the processing speed is slow, so it is not suitable for occasions that require high algorithm speed.

Contents of the invention

technical problem to be solved

The vertical blade of the mixer is a complex curved surface with a large amount of residual propellant attached to the surface, which cannot provide enough feature points. Therefore, the detection of the blade attitude cannot be completed through feature point matching and 3D reconstruction. In order to overcome its harsh detection conditions and meet its detection requirements, it is necessary to propose a simple and feasible blade attitude detection method that does not rely on a certain number of feature points and does not require 3D reconstruction.

Technical solutions

In order to solve the problems existing in the prior art, the present invention proposes a method for detecting the vertical blade attitude of a hybrid machine based on a monocular camera and a robot. This method first obtains the hand-eye calibration relationship between the robot and the monocular camera, adjusts the robot to adjust the monocular camera to a suitable position, uses the monocular camera to collect the image of the mixer blade, and performs image processing and feature extraction on it. The imaging principle of the monocular camera and the related geometric relationship can solve the angle relationship between the vertical mixer blade and the camera, and then use the calibrated camera and robot hand-eye relationship to solve the rotation angle of the blade in the robot base coordinate system. That is, the blade attitude is obtained.

A method for detecting the attitude of a vertical blade of a hybrid machine based on a monocular camera and a robot, characterized in that the steps are as follows:

Step 1: Define the image pixel coordinate system of the monocular camera as O _pix X _pix Y _pix , install the monocular camera at the end of the robot, use the checkerboard calibration board to calibrate the internal parameters of the monocular camera, and solve its internal parameter matrix, as follows:

1a. Place the checkerboard calibration board in the camera field of view, constantly change the posture of the calibration board, and collect the calibration board image;

1b. Process the image, and use the "Zhang Zhengyou calibration method" to calculate the internal parameter matrix K of the monocular camera, Among them, f is the focal length of the monocular camera, dx and dy are the length and width of a single photosensitive unit chip of the monocular camera, (u ₀ , v ₀ ) is the intersection point of the optical axis of the monocular camera and the imaging plane in the image pixel coordinate system pixel coordinates;

Step 2: Define the robot base coordinate system as O _R X _R Y _R Z _R , the robot flange coordinate system as O _F X _F Y _F Z _F , and the monocular camera coordinate system as O _C X _C Y _C Z _C , use the hand-eye The calibration method is to calibrate the coordinate transformation matrix T _cf between the monocular camera coordinate system and the flange coordinate system;

Step 3: Define the proper position for monocular camera detection as the camera coordinate system X _C O _C Y _C plane and the Y _C axis parallel to the Z _R axis of the robot base coordinate system. According to T _cf obtained in step 2, calculate when the camera reaches the proper position , the attitude transformation matrix required by the robot, adjust the monocular camera to this position by adjusting the robot;

Step 4: After step 3, the position of the monocular camera has been adjusted to a suitable position. Use the monocular camera to capture a blade image, and perform filtering, noise removal, edge detection, and feature extraction on it according to the following steps:

4a. Use the Canny edge detection algorithm to detect the edge of the image, extract the bilateral edge pixel coordinates of the measured blade rotation axis, and record them as pixel coordinate point sets {x _pix1 , y _pix1 }, {x _pix2 , y _pix2 };

4b. Extract the pixel coordinates of the characteristic shoulder point on one side of the blade, and record it as (x _pix3 , y _pix3 );

Step 5: Calculate the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis and the pixel distance Δx ₂ between the central axis of the blade rotation axis and the characteristic shoulder point on one side of the blade in the blade image, the steps are as follows:

5a. According to the imaging principle of the monocular camera, when the blade rotation axis is imaged on the image, there are two generatrix lines that become its edge contours, and the angles corresponding to the two bus lines are θ ₁ and θ ₂ . The position and radius R in space can be solved for θ ₁ and θ ₂ ;

5b. Fit the pixel coordinate point set {x _pix1 , y _pix1 }, {x _pix2 , y _pix2 } obtained in step 4 by the least square method, and obtain the bilateral edge of the blade rotation axis in the image pixel coordinates of the blade image The linear equations l ₁ and l ₂ in the system, and then calculate the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis, the unit is pixel;

5c. The linear equations l ₁ and l ₂ of the bilateral edges of the blade rotation axis obtained in step 5b in the image pixel coordinate system and the pixel coordinates of the single-side characteristic shoulder point of the blade obtained in step 4 (x _pix3 , y _pix3 ) , the pixel distance Δx ₂ ′ between the blade feature shoulder point in the blade image and the nearest edge of the blade rotation axis can be obtained, the unit is pixel;

5d. Define the distances between the projection straight line of the central axis of the blade rotation axis on the imaging plane and the projection straight lines of the two generatrices of the blade on the imaging plane as d ₁ and d ₂ respectively, and define i as the ratio of d ₁ to d ₂ , According to the imaging principle of the monocular camera, i=d ₁ /d ₂ =-cosθ ₂ /cosθ ₁ , so in the image, the pixel distance between the central axis of the blade rotation axis and the bilateral edges of the blade rotation axis is Δx ₁ /( i+1), iΔx ₁ /(i+1), Δx ₂ ′ obtained from step c, can be solved to obtain the pixel distance Δx ₂ , Δx ₂ =Δx ₂ ′+Δx ₁ /(i+1), the unit is pixel;

Step 6: Measure the distance L between the two characteristic shoulder points of the blade, and then calculate the distance L′ between the central axis of the blade rotation axis and the characteristic shoulder point of the blade, L′=L/2;

Step 7: Define the angle between the line connecting the two characteristic shoulder points of the blade and the X _C O _C Y _C plane of the camera coordinate system as the rotation angle θ′ of the blade in the camera coordinate system, and calculate θ′ according to the following steps:

7a. Define m as the distance between the projections of the bilateral contour lines of the blade rotation axis on the imaging plane, which corresponds to the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis in step 5, and n is the center of the blade rotation axis The distance between the axis and the projection of the characteristic shoulder point of the blade on the imaging plane corresponds to the pixel distance Δx ₂ between the central axis of the rotation axis of the blade and the characteristic shoulder point on one side of the blade in step 5, based on the imaging principle of the monocular camera , m/n=Δx ₁ /Δx ₂ ;

7b. Define γ as the angle between the camera optical center and the line connecting the two points of the characteristic shoulder point of the blade and the camera optical axis, define D ₁ =R/cos(π-θ ₁ ), D ₂ =R/cosθ ₂ and T=L'cosθ'-L'sinθ'tanγ, by the similar triangle theorem, m/n=(D ₁ +D ₂ )/T;

7c. Based on the imaging principle of a monocular camera, tanγ=(u ₀ -x _pix3 )d _x /f, f/d _x , u ₀ is obtained by step 1, and x _pix3 is obtained by step 5;

7d. Combining steps 7a and 7b, Δx ₁ /Δx ₂ =(D ₁ +D ₂ )/T=(R/(-cosθ ₁ )+R/(cosθ ₂ ))/(L′cosθ′-L 'sinθ'tanγ), in which, Δx ₁ and Δx ₂ are obtained by step 5, the radius R of the blade rotation axis is known, and the distance L' between the central axis of the blade rotation axis and the characteristic shoulder point of the blade is obtained by step 6. Substitute the tanγ obtained in step c, and finally solve the unique unknown θ′ through simple trigonometric function operations;

Step 8: Define the angle between the line connecting the two characteristic shoulder points of the blade and the X _R O _R Z _R plane of the robot base coordinate system as the rotation angle θ of the blade in the robot base coordinate system, and calculate θ according to the following steps:

8a. The coordinate transformation matrix T _cf between the monocular camera coordinate system and the robot flange coordinate system is obtained from step 2, and the transformation matrix T _fr between the current flange coordinate system and the robot base coordinate system is read out by the robot teach pendant. The coordinate transformation matrix between the current monocular camera coordinate system and the robot base coordinate system is solved as T _cr , T _cr =T _cf T _fr ;

8b.T _cr can be expressed as [r ₁ r ₂ r ₃ t], where r ₁ , r ₂ , and r ₃ are rotation vectors, and t is translation vector, based on the positional relationship between the current camera and robot and the principle of coordinate system rotation transformation It can be obtained that r ₁ =[-cosα 0 sinα 0] ^T , where α is the angle between the X _R axis of the robot base coordinate system and the X _C axis of the camera coordinate system, and α can be obtained by inverse trigonometric functions;

8c. Obtained from the positional relationship of each coordinate system, θ=α+θ′, wherein θ′ is obtained from step 7, α is obtained from step 8b, and finally the rotation angle θ of the blade in the robot base coordinate system is obtained, namely the solution Get blade attitude information.

Beneficial effect

A method for detecting the vertical blade attitude of a mixer based on a monocular camera and a robot proposed by the present invention does not rely too much on the feature points of the vertical blade of a mixer, avoids complicated three-dimensional reconstruction work, and only uses the shape geometry of the blade The characteristics and the principle of monocular camera imaging can solve the attitude of the blade, the calculation algorithm of the rotation angle is simple, the accuracy is high, and it can meet the application requirements. This method is suitable for attitude detection of various rotation-moving rigid bodies, and can meet the detection requirements with low cost and high precision.

Description of drawings

Figure 1 is a flow chart of attitude detection of a vertical blade of a hybrid machine based on a monocular camera and a robot;

Fig. 2 is the vertical paddle schematic diagram of mixer;

Figure 3 is a schematic diagram of the installation environment of the blade, robot and monocular camera;

Figure 4 is a schematic diagram of the imaging principle of the vertical paddle rotation axis of the mixer;

Fig. 5 is a schematic top view of the vertical paddle monocular camera imaging of the mixer;

Fig. 6 is a schematic diagram of each coordinate system and the vertical blade rotation angle of the mixer;

Among them: 1-vertical blade rotation axis of the mixer; 2-central axis of the vertical blade rotation shaft of the mixer; 3-characteristic shoulder point of the vertical blade of the mixer; 4-characteristic shoulder point of the vertical blade of the mixer; 5-robot; 6-robot flange; 7-camera mount; 8-monocular camera; 9-light source; 10-end tool; 11-blade rotation axis busbar; 12-blade rotation axis busbar; Optical center; 14-camera virtual imaging plane (symmetrical to the camera’s actual imaging plane with respect to the camera’s optical center); 15. edge of the rotation axis in the blade image; 16- edge of the rotation axis in the blade image; 17-camera optical axis; 18- Vertical mixer blade rotation angle; 19-camera coordinate system; 20-robot base coordinate system.

Detailed ways

Now in conjunction with embodiment, accompanying drawing, the present invention will be further described:

Referring to accompanying drawings 1-6, this embodiment is based on the monocular camera and the attitude detection method of the robot, which is applied to the vertical paddle of the mixer. The vertical blade rotation axis 1 of the mixer is a cylinder, located above the blade, and the blade rotates around the central axis 2 of the rotation axis 1, and the rotation angle is uncertain every time it stops; the mixer blade is a centrally symmetrical rigid body , the connection line between the characteristic shoulder point 3 and the characteristic shoulder point 4 of the blade passes through the central axis 2; the monocular camera 8 is installed on the end flange 6 of the robot 5 through the mount 7, and is used to obtain the blade image, and the light source 9 can be improved Image quality, tool 10 is used to complete the follow-up robot tasks; after obtaining the internal parameter matrix of the monocular camera and the hand-eye relationship by using the camera calibration method, the monocular camera can be adjusted to a suitable position by controlling the robot, and the blade image can be collected. The image is formed on the camera imaging plane 14, the camera optical axis 17 is perpendicular to the imaging plane 14, the distance between the camera optical center 13 and the imaging plane 14 is the focal length, and the two bus lines 11 and 12 of the blade rotation axis become the corresponding blades in the blade image Bilateral edges 15 and 16; use image processing algorithms to preprocess the blade image and extract the pixel coordinates of the blade feature shoulder point and the bilateral edge of the blade rotation axis in the image, according to the imaging principle of the monocular camera and related geometric relationships and hand-eye The calibration relationship obtains the rotation angle 18 of the blade in the robot base coordinate system 20 .

Provide the concrete steps of method in the present embodiment below:

Step 1. Install the monocular camera on the end of the robot, define the pixel coordinate system of the monocular camera image plane as O _pix X _pix Y _pix , use the checkerboard calibration board to calibrate the internal parameters of the monocular camera, and solve its internal parameter matrix, the steps are as follows :

a. Place the checkerboard calibration board in the field of view of the camera so that it is placed as close as possible to the rotation axis of the propeller, and adjust the focal length of the monocular camera so that the checkerboard on the calibration board is clearly visible and all the checkerboards are in the field of view of the camera. Change the posture of the calibration board and collect 20 images of the calibration board with different postures;

b. Process the 20 images collected by the monocular camera, and use the "Zhang Zhengyou calibration method" to calculate the internal reference matrix K of the monocular camera, Among them, f is the focal length of the monocular camera, dx and dy are the length and width of a single photosensitive unit chip of the monocular camera, (u ₀ , v ₀ ) is the intersection point of the optical axis of the monocular camera and the imaging plane in the image pixel coordinate system pixel coordinates;

Step 2. Define the robot base coordinate system as O _R X _R Y _R Z _R , the robot flange coordinate system as O _F X _F Y _F Z _F , and the monocular camera coordinate system as O _C X _C Y _C Z _C . The robot is installed on the ground. The origin of the robot base coordinate system is located at the center of the robot base. When it is at the mechanical zero position, the Z axis of the robot base coordinate system is parallel to and opposite to the direction of gravity. The coordinate transformation matrix T _cf between the monocular camera coordinate system and the flange coordinate system is calibrated through the following hand-eye calibration steps:

a. Set up a space calibration object with a known pose, control the robot to move, and the monocular camera will move accordingly to ensure that the motion form is not pure translation, and obtain the pose transformation matrix between the two space calibration objects and the monocular camera coordinate system T ₁ and T ₂ , the robot controller can obtain the pose transformation matrix T ₃ of the robot flange coordinate system before and after the movement, then the constraint equation can be obtained from the space pose transformation relationship, T ₁ T _cf T ₃ = T ₂ T _cf ;

b. Control the robot to move again to ensure that the motion form is not pure translation, and a new constraint equation can be obtained. By combining two sets of equations, the unknown matrix T _cf in the equation can be solved to complete the hand-eye calibration of the monocular camera;

Step 3. When the monocular camera collects images, the imaging plane of the camera and the edge of the imaging plane are required to be parallel to the rotation axis of the blade. The propeller is installed vertically, and the central axis of the propeller rotation axis is parallel to the robot base coordinate system Z _R , then it can be regarded as requiring that the camera coordinate system X _C O _C Y _C plane and the Y _C axis are parallel to the robot base coordinate system Z _R axis, Define this position as the proper position of the monocular camera, follow the steps below to adjust the monocular camera to this position:

a. The transformation relationship T _cf between the monocular camera coordinate system and the robot flange coordinate system has been obtained from step 2, T _cf = [RT], where R is the rotation matrix, Assuming that the order of rotation from the monocular camera coordinate system to the robot flange coordinate system is ZYX, that is, to rotate β _z , β _y , and β _x around the Z _C , Y _C , and X _C axes respectively, then the rotation matrix in T _cf R can reverse the three rotation Euler angles β _z , β _y , β _x ,

Then, when the camera is in a proper position, the included angles β ₁ , β ₂ , and β ₃ between the robot flange coordinate system and the coordinate axes of the camera coordinate system can be calculated;

b. When the X _C O _C Y _C plane of the camera coordinate system and the Y _C axis are parallel to the Z _R axis of the robot base coordinate system, the transformation matrix T _cr between the robot base coordinate system and the camera coordinate system is obtained, T _cr = T _cf T _fr and T _cf are obtained in step 2. The coordinate transformation matrix T _fr between the flange coordinate system and the robot base coordinate system is directly obtained by the robot controller. In the same step a, the respective coordinates of the robot base coordinate system and the camera coordinate system can be solved. the angle between the axes Combined with β ₁ , β ₂ , and β ₃ , the angle between the robot flange coordinate system and the coordinate axes of the base coordinate system can be solved when the camera is in a suitable position, and the robot attitude transformation amount can be calculated accordingly. By adjusting the robot attitude , so that the X _C O _C Y _C plane and the Y _C axis in the camera coordinate system are parallel to the Z _R axis of the robot base coordinate system. At this time, the position of the monocular camera has been adjusted to an appropriate position;

Step 4. After step 3, the position of the monocular camera has been adjusted to a suitable position. Use the monocular camera to collect a blade image, and perform image processing operations such as filtering and denoising, edge detection, and feature extraction on it according to the following steps:

a. Use the Canny edge detection algorithm to detect the edge of the image, extract the bilateral edge pixel coordinates of the measured blade rotation axis, and record it as a pixel coordinate point set {x _pix1 , y _pix1 }, {x _pix2 , y _pix2 };

b. Extract the pixel coordinates of the characteristic shoulder point on one side of the blade, and record it as (x _pix3 , y _pix3 );

Step 5. Calculate the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis and the pixel distance Δx ₂ between the central axis of the blade rotation axis and the characteristic shoulder point on one side of the blade in the blade image, the steps are as follows:

a. The rotation axis of the blade is a cylinder. According to the imaging principle of the monocular camera, when the rotation axis of the blade is imaged on the image, there are two generatrices that become the edge contour, and the angles corresponding to the two generatrixes are θ ₁ and θ _2. Point S is the optical center of the camera. The position and diameter D of the blade in space are known. After a series of coordinate transformations, the coordinates of the central axis of the blade’s rotation axis in the monocular camera coordinate system can be obtained, and by the steps 3. It can be seen that the direction of the center axis of the blade rotation axis is parallel to the Z _R axis of the robot base coordinate system at this time, then x _c0 and z _c0 of all points on the center axis of the blade are the same and determined, and the radius of the blade rotation axis R=D /2, θ ₁ and θ ₂ can be calculated by the following formula:

When x _c0 < 0,

When x _c0 ≥ 0,

b. The two busbars on the blade rotation axis in step a correspond to the bilateral edges of the blade rotation axis in the blade image in step 4, and the edge of the rotation axis in the blade image corresponds to the two busbars on the imaging plane projection on . The pixel coordinate point set {x _pix1 , y _pix1 }, {x _pix2 , y _pix2 } obtained in step 4 is fitted by the least square method, and the two sides of the blade rotation axis in the blade image collected in step 4 can be obtained The straight line equation l ₁ and l ₂ of the edge in the image pixel coordinate system, and then calculate the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis, the unit is pixel;

c. From the linear equations l ₁ and l ₂ of the bilateral edges of the blade rotation axis obtained in step b in the image pixel coordinate system and the pixel coordinates of the characteristic shoulder point on one side of the blade obtained in step 4 (x _pix3 , y _pix3 ) , the pixel distance Δx ₂ ′ between the blade characteristic shoulder point and the nearest edge of the blade rotation axis in the blade image collected in step 4 can be obtained, the unit is pixel;

d. Define the distances between the projection straight line of the central axis of the blade rotation axis on the imaging plane and the projection straight lines of the two generatrices of the blade on the imaging plane as d ₁ and d ₂ respectively, and define i as the ratio of d ₁ to d ₂ , According to the imaging principle of the monocular camera, i=d ₁ /d ₂ =-cosθ ₂ /cosθ ₁ , it can be obtained from the similar triangle theorem, in the image of the blade, the distance between the central axis of the blade rotation axis and the bilateral edges of the blade rotation axis The pixel distances are Δx ₁ /(i+1), iΔx ₁ /(i+1), and Δx ₂ ′ obtained from step c can be solved to obtain the central axis of the blade rotation axis and the characteristic shoulder point on one side of the blade in the image The pixel distance Δx ₂ , Δx ₂ =Δx ₂ ′+Δx ₁ /(i+1), the unit is pixel;

Step 6. Measure the distance L between the two characteristic shoulder points of the blade, and the shape of the blade is symmetrical to the center, and then calculate the distance L' between the central axis of the blade rotation axis and the characteristic shoulder point of the blade, L'=L/2;

Step 7. Define the angle between the line connecting the two characteristic shoulder points of the blade and the X _C O _C Y _C plane of the camera coordinate system as the rotation angle θ′ of the blade in the camera coordinate system, and calculate θ′ according to the following steps:

a. Define m as the distance between the projections of the bilateral contour lines of the blade rotation axis on the imaging plane, which corresponds to the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis in step 5, and n is the center of the blade rotation axis The distance between the axis and the projection of the characteristic shoulder point of the blade on the imaging plane corresponds to the pixel distance Δx ₂ between the central axis of the rotation axis of the blade and the characteristic shoulder point on one side of the blade in step 5, based on the imaging principle of the monocular camera , m/n=Δx ₁ /Δx ₂ ;

b. Define γ as the angle between the optical center of the camera and the characteristic shoulder point of the blade and the optical axis of the camera, define D ₁ =R/cos(π-θ ₁ ), D ₂ =R/cosθ ₂ and T=L'cosθ'-L'sinθ'tanγ, by the similar triangle theorem, m/n=(D ₁ +D ₂ )/T;

c. Based on the construction principle of the monocular camera, tanγ=(u ₀ -x _pix3 )d _x /f, f/d _x , u ₀ is obtained by step 1, and x _pix3 is obtained by step 5;

d. Combining steps a and b, Δx ₁ /Δx ₂ =(D ₁ +D ₂ )/T=(R/(-cosθ ₁ )+R/(cosθ ₂ ))/(L′cosθ′-L 'sinθ'tanγ), where, Δx ₁ and Δx ₂ are obtained by step 5, the radius R of the blade rotation axis is known, and the distance L' between the central axis of the blade rotation axis and the characteristic shoulder point of the blade is obtained by step 6, and Substitute the tanγ obtained in step c, and finally solve the unique unknown θ′ through simple trigonometric function operations.

θ'=cos ^-1 (((Δx ₂ /Δx ₁ )(-R/cosθ ₁ +R/cosθ ₂ ))/L')-γ;

Step 8. Define the angle between the line connecting the two characteristic shoulder points of the blade and the X _R O _R Z _R plane of the robot base coordinate system as the rotation angle θ of the blade in the robot base coordinate system, and calculate θ according to the following steps:

a. The coordinate transformation matrix T _cf between the monocular camera coordinate system and the robot flange coordinate system is obtained from step 2, and the transformation matrix T _fr between the current flange coordinate system and the robot base coordinate system is read out by the robot teach pendant. The coordinate transformation matrix between the current monocular camera coordinate system and the robot base coordinate system is solved as T _cr , T _cr =T _cf T _fr ;

bT _cr can be expressed as [r ₁ r ₂ r ₃ t], r ₁ , r ₂ , r ₃ are the rotation vector, t is the translation vector, from step 3 we can see that the monocular camera coordinate system Y _C axis and the robot base coordinate system If the Z and _R axes are parallel, it can be obtained from the principle of Cartesian coordinate system rotation transformation, r ₁ =[-cosα 0 sinα 0] ^T , where α is the distance between the robot base coordinate system X _R axis and the camera coordinate system X _C axis Angle, α can be solved by inverse trigonometric function;

c. Obtained from the positional relationship of each coordinate system and the rotation angle, θ=α+θ′, where θ′ is obtained from step 7, α is obtained from step b, and finally the rotation angle θ of the blade in the robot base coordinate system is obtained , that is, the blade attitude information is obtained.

Claims (1)

1. a hybrid machine vertical paddle attitude detection method based on monocular camera and robot, is characterized in that the steps are as follows: Step 1: Define the image pixel coordinate system of the monocular camera as O _pix X _pix Y _pix , install the monocular camera at the end of the robot, use the checkerboard calibration board to calibrate the internal parameters of the monocular camera, and solve its internal parameter matrix, as follows: 1a. Place the checkerboard calibration board in the camera field of view, constantly change the posture of the calibration board, and collect the calibration board image; 1b. Process the image, and use the "Zhang Zhengyou calibration method" to calculate the internal parameter matrix K of the monocular camera, Among them, f is the focal length of the monocular camera, dx and dy are the length and width of a single photosensitive unit chip of the monocular camera, (u ₀ , v ₀ ) is the intersection point of the optical axis of the monocular camera and the imaging plane in the image pixel coordinate system pixel coordinates; Step 2: Define the robot base coordinate system as O _R X _R Y _R Z _R , the robot flange coordinate system as O _F X _F Y _F Z _F , and the monocular camera coordinate system as O _C X _C Y _C Z _C , use the hand-eye The calibration method is to calibrate the coordinate transformation matrix T _cf between the monocular camera coordinate system and the flange coordinate system; Step 3: Define the proper position for monocular camera detection as the camera coordinate system X _C O _C Y _C plane and the Y _C axis parallel to the Z _R axis of the robot base coordinate system. According to T _cf obtained in step 2, calculate when the camera reaches the proper position , the attitude transformation matrix required by the robot, adjust the monocular camera to this position by adjusting the robot; Step 4: After step 3, the position of the monocular camera has been adjusted to a suitable position. Use the monocular camera to capture a blade image, and perform filtering, noise removal, edge detection, and feature extraction on it according to the following steps: 4a. Use the Canny edge detection algorithm to detect the edge of the image, extract the bilateral edge pixel coordinates of the measured blade rotation axis, and record them as pixel coordinate point sets {x _pix1 , y _pix1 }, {x _pix2 , y _pix2 }; 4b. Extract the pixel coordinates of the characteristic shoulder point on one side of the blade, and record it as (x _pix3 , y _pix3 ); Step 5: Calculate the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis and the pixel distance Δx ₂ between the central axis of the blade rotation axis and the characteristic shoulder point on one side of the blade in the blade image, the steps are as follows: 5a. According to the imaging principle of the monocular camera, when the blade rotation axis is imaged on the image, there are two generatrix lines that become its edge contours, and the angles corresponding to the two bus lines are θ ₁ and θ ₂ . The position and radius R in space can be solved for θ ₁ and θ ₂ ; 5b. Fit the pixel coordinate point set {x _pix1 , y _pix1 }, {x _pix2 , y _pix2 } obtained in step 4 by the least square method, and obtain the bilateral edge of the blade rotation axis in the image pixel coordinates of the blade image The linear equations l ₁ and l ₂ in the system, and then calculate the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis, the unit is pixel; 5c. The linear equations l ₁ and l ₂ of the bilateral edges of the blade rotation axis obtained in step 5b in the image pixel coordinate system and the pixel coordinates of the single-side characteristic shoulder point of the blade obtained in step 4 (x _pix3 , y _pix3 ) , the pixel distance Δx ₂ ′ between the blade feature shoulder point in the blade image and the nearest edge of the blade rotation axis can be obtained, the unit is pixel; 5d. Define the distances between the projection straight line of the central axis of the blade rotation axis on the imaging plane and the projection straight lines of the two generatrices of the blade on the imaging plane as d ₁ and d ₂ respectively, and define i as the ratio of d ₁ to d ₂ , According to the imaging principle of the monocular camera, i=d ₁ /d ₂ =-cosθ ₂ /cosθ ₁ , so in the image, the pixel distance between the central axis of the blade rotation axis and the bilateral edges of the blade rotation axis is Δx ₁ /( i+1), iΔx ₁ /(i+1), Δx ₂ ′ obtained from step c, can be solved to obtain the pixel distance Δx ₂ , Δx ₂ =Δx ₂ ′+Δx ₁ /(i+1), the unit is pixel; Step 6: Measure the distance L between the two characteristic shoulder points of the blade, and then calculate the distance L′ between the central axis of the blade rotation axis and the characteristic shoulder point of the blade, L′=L/2; Step 7: Define the angle between the line connecting the two characteristic shoulder points of the blade and the X _C O _C Y _C plane of the camera coordinate system as the rotation angle θ′ of the blade in the camera coordinate system, and calculate θ′ according to the following steps: 7a. Define m as the distance between the projections of the bilateral contour lines of the blade rotation axis on the imaging plane, which corresponds to the pixel distance Δx ₁ between the bilateral edges of the blade rotation axis in step 5, and n is the center of the blade rotation axis The distance between the axis and the projection of the characteristic shoulder point of the blade on the imaging plane corresponds to the pixel distance Δx ₂ between the central axis of the rotation axis of the blade and the characteristic shoulder point on one side of the blade in step 5, based on the imaging principle of the monocular camera , m/n=Δx ₁ /Δx ₂ ; 7b. Define γ as the angle between the camera optical center and the line connecting the two points of the characteristic shoulder point of the blade and the camera optical axis, define D ₁ =R/cos(π-θ ₁ ), D ₂ =R/cosθ ₂ and T=L'cosθ'-L'sinθ'tanγ, by the similar triangle theorem, m/n=(D ₁ +D ₂ )/T; 7c. Based on the imaging principle of a monocular camera, tanγ=(u ₀ -x _pix3 )d _x /f, f/d _x , u ₀ is obtained by step 1, and x _pix3 is obtained by step 5; 7d. Combining steps 7a and 7b, Δx ₁ /Δx ₂ =(D ₁ +D ₂ )/T=(R/(-cosθ ₁ )+R/(cosθ ₂ ))/(L′cosθ′-L 'sinθ'tanγ), in which, Δx ₁ and Δx ₂ are obtained by step 5, the radius R of the blade rotation axis is known, and the distance L' between the central axis of the blade rotation axis and the characteristic shoulder point of the blade is obtained by step 6. Substitute the tanγ obtained in step c, and finally solve the unique unknown θ′ through simple trigonometric function operations; Step 8: Define the angle between the line connecting the two characteristic shoulder points of the blade and the X _R O _R Z _R plane of the robot base coordinate system as the rotation angle θ of the blade in the robot base coordinate system, and calculate θ according to the following steps: 8a. The coordinate transformation matrix T _cf between the monocular camera coordinate system and the robot flange coordinate system is obtained from step 2, and the transformation matrix T _fr between the current flange coordinate system and the robot base coordinate system is read out by the robot teach pendant. The coordinate transformation matrix between the current monocular camera coordinate system and the robot base coordinate system is solved as T _cr , T _cr =T _cf T _fr ; 8b.T _cr can be expressed as [r ₁ r ₂ r ₃ t], where r ₁ , r ₂ , and r ₃ are rotation vectors, and t is translation vector, based on the positional relationship between the current camera and robot and the principle of coordinate system rotation transformation It can be obtained that r ₁ =[-cosα 0 sinα 0] ^T , where α is the angle between the X _R axis of the robot base coordinate system and the X _C axis of the camera coordinate system, and α can be obtained by inverse trigonometric functions; 8c. Obtained from the positional relationship of each coordinate system, θ=α+θ′, wherein θ′ is obtained from step 7, α is obtained from step 8b, and finally the rotation angle θ of the blade in the robot base coordinate system is obtained, namely the solution Get blade attitude information.