CN111267095B - Mechanical arm grabbing control method based on binocular vision - Google Patents
Mechanical arm grabbing control method based on binocular vision Download PDFInfo
- Publication number
- CN111267095B CN111267095B CN202010037021.6A CN202010037021A CN111267095B CN 111267095 B CN111267095 B CN 111267095B CN 202010037021 A CN202010037021 A CN 202010037021A CN 111267095 B CN111267095 B CN 111267095B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- coordinate system
- target
- target object
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 28
- 230000036544 posture Effects 0.000 claims abstract description 39
- 239000012636 effector Substances 0.000 claims abstract description 34
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000005484 gravity Effects 0.000 claims description 14
- 238000013519 translation Methods 0.000 claims description 12
- 238000002474 experimental method Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 claims description 3
- 239000010931 gold Substances 0.000 claims description 3
- 229910052737 gold Inorganic materials 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 9
- 238000001514 detection method Methods 0.000 abstract description 5
- 239000003086 colorant Substances 0.000 abstract description 3
- 238000000605 extraction Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000001678 irradiating effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1612—Programme controls characterised by the hand, wrist, grip control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1648—Programme controls characterised by the control loop non-linear control combined or not with linear control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Nonlinear Science (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
The invention relates to the technical field of a kinova mechanical arm and ZED vision, and discloses a mechanical arm grabbing control method based on binocular vision, which comprises the following steps: (1) the method comprises the following steps of (1) establishing an experimental platform, (2) identifying the position and the posture of a target when the target is placed in an inclined mode, (3) identifying the position and the posture of the target when the target is placed horizontally, (4) identifying and grabbing the target by using an end effector of a mechanical arm, and (5) enabling the mechanical arm to reach a specified joint angle state after grabbing the target. The invention has the following advantages: firstly, the attitude angle of the target object is estimated, the deviation of the included angle between the direction vector and the coordinate axis plane is within-5 to +5 degrees, and the detection accuracy is greatly improved. Secondly, the interference of interference items with the same or similar colors on the identification of the target object can be avoided by adopting the color and the shape. Thirdly, the two mechanical arms are set to grab the left and right target objects, and the effect of the postures of the left and right mechanical arms for grabbing the target objects is finally obtained.
Description
Technical Field
The invention relates to a mechanical arm grabbing control method based on binocular vision, and belongs to the technical field of a kinova mechanical arm and ZED vision.
Background
Binocular vision pose measurement is taken as an important target pose mode, but certain difficulty exists in measuring the pose of an obliquely placed target object, and the existing methods comprise the following steps: a circle extraction method and a feature point extraction method.
Round section extraction: in the existing method, 3 industrial CCD cameras are used for extracting the circle centers of the upper and lower sections of a cylindrical target object, a coordinate system is defined, and the included angle between the connecting line of the circle centers of the two sections and the coordinate system is calculated to describe the position and the posture.
The characteristic point extraction method comprises the following steps: two cases are distinguished: firstly, manually setting characteristic points; secondly, laser irradiation is used for acquiring the characteristic points. In the first case, a pyramid prism is set on the surface of the target object as a feature point, and the position and the posture of the target object are obtained by irradiating the prism with laser with a certain wavelength. In the second case, the feature points are artificially created by irradiating the surface of the part with laser rays, and the accuracy of the method greatly depends on the accuracy and stability of the emitted laser, thereby increasing the detection cost.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a mechanical arm grabbing control method based on binocular vision. Due to the fact that the contour extraction precision of the circular section is not high, the circle center extraction has large deviation. In practical situations, there are some cases that the circular cross section is blocked, and the like, which all make the circle center line method to estimate the posture of the cylinder impractical. In order to solve the problems of difficulty in extracting the section circle, low precision and the like, the invention provides a method for extracting the foreground to obtain the arc vertex of the section of the target object, the posture of the target object is estimated according to the coordinate difference of the vertex, and finally the mechanical arm is controlled to be capable of flexibly grabbing the target object like a human hand.
In order to achieve the purpose of the invention and solve the problems existing in the prior art, the invention adopts the technical scheme that: a mechanical arm grabbing control method based on binocular vision comprises the following steps:
step 1, an experiment platform is built, a binocular vision camera is installed at a position 1-1.5m above the experiment platform, the binocular vision camera is started, a kinova mechanical arm is started, a left mechanical arm model and a right mechanical arm model urdf model are loaded, the models describe the position and posture relation of each joint of the kinova mechanical arm, the left mechanical arm is installed on the left hand side of a horizontal desktop, the bottom coordinate system of the left mechanical arm is defined as left, the right mechanical arm is installed on the right hand side of the horizontal desktop, the bottom coordinate system of the right mechanical arm is defined as right, and the distance between the left mechanical arm and the right mechanical arm is 0.8-1.2 m. A root coordinate system is defined as root in the middle position of the left mechanical arm and the right mechanical arm, a world coordinate system of the binocular vision camera is defined as map, the root is used as the root coordinate system, the left, right and map coordinate positions and postures are determined by translation and rotation, the origin of the root coordinate system is used as a central point, a rotation matrix rotating to the bottom coordinate system of the left mechanical arm is R1, a translation matrix is T1, a rotation matrix rotating to the bottom coordinate system of the right mechanical arm is R2, a translation matrix T2, a rotation matrix rotating to the coordinate system of the binocular vision camera is R3, a translation matrix is T3, and the coordinate system is described by equations (1) to (3),
left=R1*root+T1 (1)
right=R2*root+T2 (2)
map=R3*root+T3 (3)
(a) subscribing an RGB image published by a binocular vision camera, carrying out binarization on the image, manually setting a pixel frame due to more foreign objects outside the image, and extracting a foreground in the frame by using a grabCut algorithm in opencv, wherein the iteration number is set to be 7-12;
(b) extracting foreground image pixel pi(u, v), i belongs to N (N is the number of pixel points extracted from the foreground) and the depth image are matched to obtain the corresponding world coordinate P in the world coordinate systemi=[XW YW ZW]TI ∈ N. N PiThe mapping relation of the world coordinates mapped from all the pixel coordinate systems to the binocular vision camera stored in the 1 st container is described by equation (4),
wherein z is a binocular vision camera depth value,f is the focal length of the binocular vision camera, dx is the physical distance corresponding to the horizontal unit pixel in mm, dy is the physical distance corresponding to the vertical unit pixel in mm, and the parameter u in the formula0Is the transverse midpoint of a pixel of the image, v0Is the longitudinal midpoint of an image pixel. Storing the converted world coordinates in a 2 nd container, traversing the whole 2 nd container, and solving the element world coordinate P with the minimum x value in all elements1=[X,Y,Z],X∈min{XWThe element represents the highest of the two circular arcsVertex P1With P1Is the central point;
(c) traversing the 2 nd container by taking the bottle length L as a constraint condition to find out the central point P1All points with the distance satisfying the constraint condition of L length are stored in a 3 rd container, and then a target P corresponding to the minimum value of the x-axis coordinate is searched from the 3 rd container2I.e. the highest vertex P of the rear arc2The direction vector v, calculated by equation (5),
v=P1-P2 (5)
(d) calculating the center of gravity Q3 of the target object, and setting the point Q2 as P in the substep (c)1And P2Wherein the radius of the cross-section of the bottle is r, the distance between Q2 and the point Q directly below is h, Q2, Q3 form a geometric relationship in the form of a right triangle and satisfy Q3-Q ═ kv, k ≠ 0, v ≠ kvx,vy,vzThe projection of the direction vector v on the x-axis, the y-axis and the z-axis of the world coordinate system of the binocular vision camera is described by the formulas (6) to (8),
(e) the reference system of the direction vector v is a binocular vision camera world coordinate system map, the direction vector v of the right target object is converted into a right mechanical arm bottom coordinate system right, the direction vector is v2, the conversion formula is described by an equation (9),
wherein,the angle between the direction vector v2 and each coordinate plane of right is calculated for the rotation matrix of converting the world coordinate system map of the binocular vision camera to the coordinate system right at the bottom of the right mechanical arm, which is described by the formula (10),
in the formula, v2x,v2y,v2zV2 are respectively projected on an x axis, a y axis and a z axis under a right mechanical arm base coordinate system, alpha is an included angle between a direction vector v2 and an x-y plane, beta is an included angle between a direction vector v2 and the x-z plane, and chi is an included angle between a direction vector v2 and the y-z plane;
step 3, recognizing the position and the posture of the object when the object is horizontally placed, which specifically comprises the following substeps:
(a) a color segmentation mode is required to be used for extracting a binary image of a left target object, because interference of irrelevant objects with the same color exists in the process, the binary image is also extracted;
(b) in order to solve the problem that the same color interferes with the identification of the target object, the shape of the target object is taken as an object identification auxiliary condition, the extracted image is screened by utilizing the shape characteristic, and a binary image with a quadrilateral characteristic is reserved;
(c) using a polygon approximation contour function cv in opencv to approximate approxColy (), and obtaining a two-dimensional array M multiplied by N, wherein M represents the number of polygon groups, and N represents a set of each group of polygonal boundary pixel points;
(d) judging whether the target object is a quadrangle or not, wherein the target object to be extracted is a quadrangle, but a certain deviation may exist in the actual engineering, the number of the edges of the target object is selected to be 3-5, and the target object can be considered to be in accordance with the shape characteristics of the target object in the range and is reserved;
(e) solving all shape information by using a findContours () function, and storing a vector of the outline;
(f) solving the number of edges of each contour by using an approx PolyDP () function, traversing the whole 1 st container, deleting interferents with the number of edges not being 3-5, keeping elements with approximate quadrilateral shapes in the 1 st container, drawing a frame by using a polylines () function, solving the pixel coordinate of a gravity center point gold, and calculating the position coordinate of the gravity center point in a world coordinate system of a binocular vision camera by using depth image matching;
(g) after the contour information of the target object is determined, setting four vertexes of a rectangular frame as A, B, C and D, if AB is the longest side, selecting the difference of coordinates of the vertexes of the AB side as a direction vector, converting the direction vector v of the left target object into the lower v1 of a left mechanical arm base coordinate system, and calculating the included angle between the direction vector and the x-z plane of the left mechanical arm base coordinate system to beDescribed by the formula (11),
wherein, v1x,v1y,v1zRespectively are the projections of v1 on the x axis, the y axis and the z axis under the coordinate system of the left mechanical arm base;
the right target object in the experimental state is placed obliquely to the table top, the vector of the direction under the bottom coordinate system of the right mechanical arm is v2, the included angle between v2 and the x-y plane is alpha, the included angle sigma between the z axis of the end effector of the right mechanical arm and the z axis of the bottom coordinate system of the right mechanical arm is described by solving the equations (13) and (14),
wherein delta is latitude value in spherical coordinate, v2 is included angle beta of z-x plane of bottom coordinate system of right mechanical arm, right mechanical arm end effector rotates angle Gr around joint axis, which is described by formulas (15) and (16),
after attitude control is finished, the end effector of the mechanical arm needs to be controlled to reach a target position, wherein the azimuth angle is defined as a horizontal included angle between a line starting from the positive direction of an x axis and a line from a clockwise direction to a target direction, and because the azimuth angle theta is [0,2 pi ], the (x, y) needs to be judged to be positioned in a quadrant in a four quadrant, and the azimuth angle theta is calculated through a formula (17);
in order to avoid collision with a target object during grasping, a pre-grasping process is designed, the position and posture of grasping are designed in advance, and the description is given by equation (18),
in the formula (x)goal,ygoal,zgoal)TIs the position coordinate of the target gravity center point relative to the bottom coordinate system of the mechanical arm, (x ', y ', z ')TThe coordinate point is a coordinate point of a pre-grabbing position relative to a coordinate system at the bottom of the mechanical arm, and L is a distance set manually; theta is an included angle between the x-y plane projection of the target point in the coordinate system of the mechanical arm and the x axis, and sigma is an included angle between the z axis of the end effector of the mechanical arm and the z axis of the coordinate system at the bottom of the mechanical arm; obtaining a preset target position and a preset target posture by using the formulas (11) to (18), and controlling the end effector of the mechanical arm to grab the target object in the optimal posture;
and 5, after the mechanical arm grabs the target, the mechanical arm action client sends the specified joint angle to the action server, and the action is finished after the execution.
The invention has the beneficial effects that: a mechanical arm grabbing control method based on binocular vision comprises the following steps:
(1) the method comprises the following steps of (1) establishing an experimental platform, (2) identifying the position and the posture of a target when the target is placed in an inclined mode, (3) identifying the position and the posture of the target when the target is placed horizontally, (4) identifying and grabbing the target by using an end effector of a mechanical arm, and (5) enabling the mechanical arm to reach a specified joint angle state after grabbing the target. Compared with the prior art, the invention has the following advantages: firstly, extracting the vertex coordinates of front and rear circular arcs from the foreground of the target object, and estimating the direction vector of the target object by using the difference value of the coordinates, thereby estimating the attitude angle of the target object, wherein the deviation of the included angle between the direction vector and the coordinate axis plane is within-5 to +5 degrees, and the detection accuracy is greatly improved. Secondly, the interference of interference items with the same or similar colors on the identification of the target object can be avoided by adopting the colors and the shapes, the influence of the interference items on the target detection is overcome, the center of gravity is stable, and the position and the posture information of the target object can be stably detected. Thirdly, the double mechanical arms are set to grab the left and right target objects, and the pose of the left target object can guide the left mechanical arm to grab the target object in the optimal pose. The pose of the right target object can guide the right mechanical arm to grab the target object in the optimal pose, and the effect of grabbing the target object pose by the left mechanical arm and the right mechanical arm is finally obtained.
Drawings
FIG. 1 is a flow chart of the method steps of the present invention.
Fig. 2 is a diagram of the detection effect of the posture of the right target object.
In the figure: (a) the method comprises the following steps of (a) obtaining a right target object attitude image, (b) selecting a rectangular frame and extracting a target object foreground elliptical image only in the frame, (c) obtaining a geometric relation image of a right target under a world coordinate system of a binocular vision camera, and (d) obtaining a result image of a right target object direction vector.
Fig. 3 is a diagram of the effect of detecting the posture of the left target object.
In the figure: (a) the method comprises the following steps of (a) obtaining a left target object posture graph, (b) obtaining a color + shape recognition effect graph, (c) obtaining a geometric relation graph of a left target under a left mechanical arm bottom coordinate system, and (d) obtaining a result graph of a left target object direction vector.
Fig. 4 is a coordinate system conversion diagram of the left and right robot arms.
Fig. 5 is an effect diagram of the left and right mechanical arms grabbing the target object in a special posture.
In the figure: (a) the effect graph of the left mechanical arm grabbing the target object in the special posture is shown, and the effect graph of the right mechanical arm grabbing the target object in the special posture is shown.
Fig. 6 is an effect diagram of the left and right robot arms after the completion of gripping two target objects.
Detailed Description
The invention will be further explained with reference to the drawings.
As shown in fig. 1, a mechanical arm grabbing control method based on binocular vision includes the following steps:
step 1, an experiment platform is built, a binocular vision camera is installed at a position 1-1.5m above the experiment platform, the binocular vision camera is started, a kinova mechanical arm is started, a left mechanical arm model and a right mechanical arm model urdf model are loaded, the models describe the position and posture relation of each joint of the kinova mechanical arm, the left mechanical arm is installed on the left hand side of a horizontal desktop, the bottom coordinate system of the left mechanical arm is defined as left, the right mechanical arm is installed on the right hand side of the horizontal desktop, the bottom coordinate system of the right mechanical arm is defined as right, and the distance between the left mechanical arm and the right mechanical arm is 0.8-1.2 m. A root coordinate system is defined as root in the middle position of the left mechanical arm and the right mechanical arm, a world coordinate system of the binocular vision camera is defined as map, the root is used as the root coordinate system, the left, right and map coordinate positions and postures are determined by translation and rotation, the origin of the root coordinate system is used as a central point, a rotation matrix rotating to the bottom coordinate system of the left mechanical arm is R1, a translation matrix is T1, a rotation matrix rotating to the bottom coordinate system of the right mechanical arm is R2, a translation matrix T2, a rotation matrix rotating to the coordinate system of the binocular vision camera is R3, a translation matrix is T3, and the coordinate system is described by equations (1) to (3),
left=R1*root+T1 (1)
right=R2*root+T2 (2)
map=R3*root+T3 (3)
(a) subscribing an RGB image published by a binocular vision camera, carrying out binarization on the image, manually setting a pixel frame due to more foreign objects outside the image, and extracting a foreground in the frame by using a grabCut algorithm in opencv, wherein the iteration number is set to be 7-12;
(b) extracting foreground image pixel pi(u, v), i belongs to N (N is the number of pixel points extracted from the foreground) and the depth image are matched to obtain the corresponding world coordinate P in the world coordinate systemi=[XW YW ZW]TI ∈ N. N PiThe mapping relation of the world coordinates mapped from all the pixel coordinate systems to the binocular vision camera stored in the 1 st container is described by equation (4),
wherein z is a binocular vision camera depth value,f is the focal length of the binocular vision camera, dx is the physical distance corresponding to the horizontal unit pixel in mm, dy is the physical distance corresponding to the vertical unit pixel in mmIn the formula, parameter u0Is the transverse midpoint of a pixel of the image, v0Is the longitudinal midpoint of an image pixel. Storing the converted world coordinates in a 2 nd container, traversing the whole 2 nd container, and solving the element world coordinate P with the minimum x value in all elements1=[X,Y,Z],X∈min{XWRepresents the highest vertices P of two circular arcs1With P1Is the central point;
(c) traversing the 2 nd container by taking the bottle length L as a constraint condition to find out the central point P1All points with the distance satisfying the constraint condition of L length are stored in a 3 rd container, and then a target P corresponding to the minimum value of the x-axis coordinate is searched from the 3 rd container2I.e. the highest vertex P of the rear arc2The direction vector v, calculated by equation (5),
v=P1-P2 (5)
(d) calculating the center of gravity Q3 of the target object, and setting the point Q2 as P in the substep (c)1And P2Wherein the radius of the cross-section of the bottle is r, the distance between Q2 and the point Q directly below is h, Q2, Q3 form a geometric relationship in the form of a right triangle and satisfy Q3-Q ═ kv, k ≠ 0, v ≠ kvx,vy,vzThe projection of the direction vector v on the x-axis, the y-axis and the z-axis of the world coordinate system of the binocular vision camera is described by the formulas (6) to (8),
(e) the reference system of the direction vector v is a binocular vision camera world coordinate system map, the direction vector v of the right target object is converted into a right mechanical arm bottom coordinate system right, the direction vector is v2, the conversion formula is described by an equation (9),
wherein,the angle between the direction vector v2 and each coordinate plane of right is calculated for the rotation matrix of converting the world coordinate system map of the binocular vision camera to the coordinate system right at the bottom of the right mechanical arm, which is described by the formula (10),
in the formula, v2x,v2y,v2zV2 are respectively projected on an x axis, a y axis and a z axis under a right mechanical arm base coordinate system, alpha is an included angle between a direction vector v2 and an x-y plane, beta is an included angle between a direction vector v2 and the x-z plane, and chi is an included angle between a direction vector v2 and the y-z plane;
step 3, recognizing the position and the posture of the object when the object is horizontally placed, which specifically comprises the following substeps:
(a) a color segmentation mode is required to be used for extracting a binary image of a left target object, because interference of irrelevant objects with the same color exists in the process, the binary image is also extracted;
(b) in order to solve the problem that the same color interferes with the identification of the target object, the shape of the target object is taken as an object identification auxiliary condition, the extracted image is screened by utilizing the shape characteristic, and a binary image with a quadrilateral characteristic is reserved;
(c) using a polygon approximation contour function cv in opencv to approximate approxColy (), and obtaining a two-dimensional array M multiplied by N, wherein M represents the number of polygon groups, and N represents a set of each group of polygonal boundary pixel points;
(d) judging whether the target object is a quadrangle or not, wherein the target object to be extracted is a quadrangle, but a certain deviation may exist in the actual engineering, the number of the edges of the target object is selected to be 3-5, and the target object can be considered to be in accordance with the shape characteristics of the target object in the range and is reserved;
(e) solving all shape information by using a findContours () function, and storing a vector of the outline;
(f) solving the number of edges of each contour by using an approx PolyDP () function, traversing the whole 1 st container, deleting interferents with the number of edges not being 3-5, keeping elements with approximate quadrilateral shapes in the 1 st container, drawing a frame by using a polylines () function, solving the pixel coordinate of a gravity center point gold, and calculating the position coordinate of the gravity center point in a world coordinate system of a binocular vision camera by using depth image matching;
(g) after the contour information of the target object is determined, setting four vertexes of a rectangular frame as A, B, C and D, if AB is the longest side, selecting the difference of coordinates of the vertexes of the AB side as a direction vector, converting the direction vector v of the left target object into the lower v1 of a left mechanical arm base coordinate system, and calculating the included angle between the direction vector and the x-z plane of the left mechanical arm base coordinate system to beDescribed by the formula (11),
wherein, v1x,v1y,v1zRespectively are the projections of v1 on the x axis, the y axis and the z axis under the coordinate system of the left mechanical arm base;
the right target object in the experimental state is placed obliquely to the table top, the vector of the direction under the bottom coordinate system of the right mechanical arm is v2, the included angle between v2 and the x-y plane is alpha, the included angle sigma between the z axis of the end effector of the right mechanical arm and the z axis of the bottom coordinate system of the right mechanical arm is described by solving the equations (13) and (14),
wherein delta is latitude value in spherical coordinate, v2 is included angle beta of z-x plane of bottom coordinate system of right mechanical arm, right mechanical arm end effector rotates angle Gr around joint axis, which is described by formulas (15) and (16),
after attitude control is finished, the end effector of the mechanical arm needs to be controlled to reach a target position, wherein the azimuth angle is defined as a horizontal included angle between a line starting from the positive direction of an x axis and a line from a clockwise direction to a target direction, and because the azimuth angle theta is [0,2 pi ], the (x, y) needs to be judged to be positioned in a quadrant in a four quadrant, and the azimuth angle theta is calculated through a formula (17);
in order to avoid collision with a target object during grasping, a pre-grasping process is designed, the position and posture of grasping are designed in advance, and the description is given by equation (18),
in the formula (x)goal,ygoal,zgoal)TIs the position coordinate of the target gravity center point relative to the bottom coordinate system of the mechanical arm, (x ', y ', z ')TThe coordinate point is a coordinate point of a pre-grabbing position relative to a coordinate system at the bottom of the mechanical arm, and L is a distance set manually; theta is an included angle between the x-y plane projection of the target point in the coordinate system of the mechanical arm and the x axis, and sigma is an included angle between the z axis of the end effector of the mechanical arm and the z axis of the coordinate system at the bottom of the mechanical arm; obtaining a preset target position and a preset target posture by using the formulas (11) to (18), and controlling the end effector of the mechanical arm to grab the target object in the optimal posture;
and 5, after the mechanical arm grabs the target, the mechanical arm action client sends the specified joint angle to the action server, and the action is finished after the execution.
Claims (1)
1. A mechanical arm grabbing control method based on binocular vision is characterized by comprising the following steps:
step 1, building an experiment platform, installing a binocular vision camera at a position 1-1.5m above the experiment platform, starting the binocular vision camera, starting a kinova mechanical arm and loading a left mechanical arm model urdf and a right mechanical arm model urdf, wherein the model describes the position and posture relation of each joint of the kinova mechanical arm, the left mechanical arm is installed at the left hand side of a horizontal desktop, the bottom coordinate system of the left mechanical arm is defined as left, the right mechanical arm is installed at the right hand side of the horizontal desktop, the bottom coordinate system of the right mechanical arm is defined as right, the distance between the left mechanical arm and the right mechanical arm is 0.8-1.2m, the root coordinate system is defined as root at the middle position of the left mechanical arm and the right mechanical arm, the world coordinate system of the binocular vision camera is defined as map, the root is used as the root coordinate system, the coordinate positions and postures of the left, the right and the map are determined by translation and rotation, the origin of the root coordinate system is used as a central point, a rotation matrix rotated to the bottom coordinate system of the left mechanical arm is R1, the translation matrix is T1, a rotation matrix rotated to the bottom coordinate system of the right robot arm is R2, a translation matrix T2, a rotation matrix rotated to the coordinate system of the binocular vision camera is R3, and a translation matrix is T3, and is described by equations (1) to (3),
left=R1*root+T1 (1)
right=R2*root+T2 (2)
map=R3*root+T3 (3)
step 2, recognizing the position and the posture of the right target object when the right target object is obliquely placed, and the method specifically comprises the following substeps:
(a) subscribing an RGB image published by a binocular vision camera, carrying out binarization on the image, manually setting a pixel frame due to more foreign objects outside the image, and extracting a foreground in the frame by using a grabCut algorithm in opencv, wherein the iteration number is set to be 7-12;
(b) extracting foreground image pixel pi(u, v), i belongs to N, and is matched with the depth image to obtain corresponding world coordinate P in a world coordinate systemi=[XW YW ZW]TI belongs to N, N is the number of pixel points extracted from the foreground, and N is PiThe mapping relation of the world coordinates mapped from all the pixel coordinate systems to the binocular vision camera stored in the 1 st container is described by equation (4),
wherein z is a binocular vision camera depth value,f is the focal length of the binocular vision camera, dx is the physical distance corresponding to the horizontal unit pixel in mm, dy is the physical distance corresponding to the vertical unit pixel in mm, and the parameter u in the formula0Is the transverse midpoint of a pixel of the image, v0Storing the converted world coordinates in a 2 nd container for the longitudinal midpoint of the image pixel, traversing the whole 2 nd container, and solving the world coordinate P of the element with the minimum x value in all the elements1=[X,Y,Z],X∈min{XWRepresents the highest vertices P of two circular arcs1With P1Is the central point;
(c) traversing the 2 nd container by taking the length Y of the right target object as a constraint condition to find out the distance from the central point P1All points with the distance satisfying the length Y as the constraint condition are stored in a 3 rd container, and then a target P corresponding to the minimum value of the x-axis coordinate is searched from the 3 rd container2I.e. the highest vertex P of the rear arc2The direction vector v, calculated by equation (5),
v=P1-P2 (5)
(d) calculating the center of gravity Q3 of the right target object, and setting the point Q2 as P in the sub-step (c)1And P2Wherein the radius of the right target section is r, the distance between Q2 and the right lower point Q is h, Q2, Q3 form the geometric relationship of a right triangle and satisfy Q3-Q ═ kv, k ≠ 0, v ≠ kvx,vy,vzThe projection of the direction vector v on the x-axis, the y-axis and the z-axis of the world coordinate system of the binocular vision camera is described by the formulas (6) to (8),
(e) the reference system of the direction vector v is a binocular vision camera world coordinate system map, the direction vector v of the right target object is converted into a right mechanical arm bottom coordinate system right, the direction vector is v2, the conversion formula is described by an equation (9),
wherein,the angle between the direction vector v2 and each coordinate plane of right is calculated for the rotation matrix of converting the world coordinate system map of the binocular vision camera to the coordinate system right at the bottom of the right mechanical arm, which is described by the formula (10),
in the formula, v2x,v2y,v2zRespectively projecting v2 on an x axis, a y axis and a z axis under a right mechanical arm bottom coordinate system, wherein alpha is an included angle between a direction vector v2 and an x-y plane, beta is an included angle between a direction vector v2 and the x-z plane, and chi is an included angle between a direction vector v2 and the y-z plane;
step 3, recognizing the position and the posture of the left object when the left object is horizontally placed, and specifically comprising the following substeps:
(a) a color segmentation mode is required to be used for extracting a binary image of a left target object, because interference of irrelevant objects with the same color exists in the process, the binary image is also extracted;
(b) in order to solve the problem that the same color interferes with the identification of the left target object, the shape of the left target object is taken as an object identification auxiliary condition, the extracted image is screened by utilizing shape characteristics, and a binary image with quadrilateral characteristics is reserved;
(c) using a polygon in opencv to approximate a contour function cv:approxpol (), obtaining a two-dimensional array M multiplied by N1, wherein M represents the number of polygon groups, and N1 represents a set of each group of polygonal boundary pixels;
(d) judging whether the object is a quadrangle or not, wherein the number of the edges of the left object is 3-5 because the left object to be extracted is a quadrangle, and the shape characteristics of the left object can be considered to be met in the range and are reserved;
(e) solving all shape information by using a findContours () function, and storing a vector of the outline;
(f) solving the number of edges of each contour by using an approx PolyDP () function, traversing the whole 1 st container, deleting interferents with the number of edges not being 3-5, keeping elements with approximate quadrilateral shapes in the 1 st container, drawing a frame by using a polylines () function, solving the pixel coordinate of a gravity center point gold, and calculating the position coordinate of the gravity center point in a world coordinate system of a binocular vision camera by using depth image matching;
(g) after the contour information of the left target object is determined, setting four vertexes of a rectangular frame as A, B, C and D, if AB is the longest side, selecting the difference of coordinates of the vertexes of the AB sides as a direction vector, converting the direction vector v of the left target object into the lower v1 of a bottom coordinate system of the left mechanical arm, and calculating the included angle between the direction vector and the x-z plane of the bottom coordinate system of the left mechanical arm to beDescribed by the formula (11),
wherein, v1x,v1y,v1zRespectively are the projections of v1 on the x axis, the y axis and the z axis under the coordinate system of the bottom of the left mechanical arm;
step 4, identifying and grabbing a left target object by the left mechanical arm end effector and identifying and grabbing a right target object by the right mechanical arm end effector, horizontally placing the position corresponding to the left target object in an experimental state, wherein the included angle between the left target object and the horizontal plane is zero, solving an included angle delta between a direction vector v1 and an x-z plane of a left mechanical arm bottom coordinate system by using a formula (11), wherein the included angle sigma between a z axis of the left mechanical arm end effector and the z axis of the left mechanical arm bottom coordinate system is 170-180 degrees, controlling the rotation angle Gr of the left mechanical arm end effector around a joint axis thereof, and describing the rotation angle Gr through a formula (12),
the right target object in the experimental state is placed obliquely to the table top, the vector of the direction under the bottom coordinate system of the right mechanical arm is v2, the included angle between v2 and the x-y plane is alpha, the included angle sigma between the z axis of the end effector of the right mechanical arm and the z axis of the bottom coordinate system of the right mechanical arm is described by solving the equations (13) and (14),
wherein delta is latitude value in spherical coordinate, an included angle beta between v2 and a z-x plane of a coordinate system at the bottom of the right mechanical arm, a rotation angle Gr of the right mechanical arm end effector around a joint axis is described by formulas (15) and (16),
after the attitude control is finished, the left mechanical arm end effector is required to be controlled to reach the left target position, the right mechanical arm end effector is required to reach the right target position, wherein the azimuth angle is defined as the horizontal included angle between the direction lines of the left mechanical arm end effector and the right mechanical arm end effector from the positive direction of the x axis to the left target object in the clockwise direction, the (x, y) is required to be judged to be positioned in the quadrant in the four quadrants as the azimuth angle theta is [0,2 pi ], and the azimuth angle theta is calculated through a formula (17);
in order to avoid collision with a left target and a right target in the grabbing process, a left mechanical arm grabbing left target pre-grabbing process is designed, a right mechanical arm grabbing right target pre-grabbing process is designed, the grabbing position and posture are designed in advance, and description is given by an equation (18),
in the formula (x)goal,ygoal,zgoal)TThe position coordinates of the gravity center point of the left target object relative to the bottom coordinate system of the left mechanical arm or the position coordinates of the gravity center point of the right target object relative to the bottom coordinate system of the right mechanical arm, (x ', y ', z ')TObtaining a preset left target position and posture and a right target position and posture by using formulas (11) to (18) for a coordinate point of a pre-grabbing position of a left target relative to a left mechanical arm bottom coordinate system or a coordinate point of a pre-grabbing position of a right target relative to a right mechanical arm bottom coordinate system, wherein L is a distance set manually, and σ is an included angle between a z-axis of a left mechanical arm end effector and a z-axis of the left mechanical arm bottom coordinate system or an included angle between a z-axis of a right mechanical arm end effector and a z-axis of the right mechanical arm bottom coordinate system, controlling the left mechanical arm end effector to grab the left target in an optimal posture, and controlling the right mechanical arm end effector to grab the right target in an optimal posture;
and 5, the left mechanical arm captures a left object and then reaches a state of an appointed joint angle, the right mechanical arm captures a right object and then reaches a state of the appointed joint angle, the left mechanical arm action client sends the appointed joint angle to the action server, the right mechanical arm action client sends the appointed joint angle to the action server, and the action is finished after the execution.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010037021.6A CN111267095B (en) | 2020-01-14 | 2020-01-14 | Mechanical arm grabbing control method based on binocular vision |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010037021.6A CN111267095B (en) | 2020-01-14 | 2020-01-14 | Mechanical arm grabbing control method based on binocular vision |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111267095A CN111267095A (en) | 2020-06-12 |
CN111267095B true CN111267095B (en) | 2022-03-01 |
Family
ID=70994170
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010037021.6A Active CN111267095B (en) | 2020-01-14 | 2020-01-14 | Mechanical arm grabbing control method based on binocular vision |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111267095B (en) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111751136A (en) * | 2020-06-29 | 2020-10-09 | 伯肯森自动化技术(上海)有限公司 | POS machine test system based on binocular vision subassembly |
CN111814634B (en) * | 2020-06-29 | 2023-09-08 | 北京百度网讯科技有限公司 | Real-time distance determining method, device, equipment and medium |
CN112667823B (en) * | 2020-12-24 | 2022-11-01 | 西安电子科技大学 | Semantic analysis method and system for task execution sequence of mechanical arm and computer readable medium |
CN112894815B (en) * | 2021-01-25 | 2022-09-27 | 西安工业大学 | Method for detecting optimal position and posture for article grabbing by visual servo mechanical arm |
CN113768640B (en) * | 2021-11-09 | 2022-02-08 | 极限人工智能有限公司 | Method and device for determining working pose of mechanical arm |
CN114516045B (en) * | 2021-11-25 | 2023-01-20 | 苏州永固智能科技有限公司 | Unmanned storehouse mechanical arm control method and system based on Internet of things |
CN114485397A (en) * | 2022-01-11 | 2022-05-13 | 重庆固高科技长江研究院有限公司 | Linear laser scanning state control method based on movement track of moving mechanism |
CN115256019B (en) * | 2022-06-25 | 2023-07-14 | 北京建工集团有限责任公司 | Automatic assembling and aligning device for support plates |
CN117163421B (en) * | 2023-11-03 | 2024-01-23 | 山东新华医疗器械股份有限公司 | Multi-arm cooperation intelligent packaging robot for disinfection supply center |
CN117464692B (en) * | 2023-12-27 | 2024-03-08 | 中信重工机械股份有限公司 | Lining plate grabbing mechanical arm control method based on structured light vision system |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05288884A (en) * | 1992-04-13 | 1993-11-05 | Toshiba Corp | Robot operated plant |
US5887121A (en) * | 1995-04-21 | 1999-03-23 | International Business Machines Corporation | Method of constrained Cartesian control of robotic mechanisms with active and passive joints |
CN102902271A (en) * | 2012-10-23 | 2013-01-30 | 上海大学 | Binocular vision-based robot target identifying and gripping system and method |
CN108582075A (en) * | 2018-05-10 | 2018-09-28 | 江门市思远信息科技有限公司 | A kind of intelligent robot vision automation grasping system |
CN109102525A (en) * | 2018-07-19 | 2018-12-28 | 浙江工业大学 | A kind of mobile robot follow-up control method based on the estimation of adaptive pose |
-
2020
- 2020-01-14 CN CN202010037021.6A patent/CN111267095B/en active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05288884A (en) * | 1992-04-13 | 1993-11-05 | Toshiba Corp | Robot operated plant |
US5887121A (en) * | 1995-04-21 | 1999-03-23 | International Business Machines Corporation | Method of constrained Cartesian control of robotic mechanisms with active and passive joints |
CN102902271A (en) * | 2012-10-23 | 2013-01-30 | 上海大学 | Binocular vision-based robot target identifying and gripping system and method |
CN108582075A (en) * | 2018-05-10 | 2018-09-28 | 江门市思远信息科技有限公司 | A kind of intelligent robot vision automation grasping system |
CN109102525A (en) * | 2018-07-19 | 2018-12-28 | 浙江工业大学 | A kind of mobile robot follow-up control method based on the estimation of adaptive pose |
Also Published As
Publication number | Publication date |
---|---|
CN111267095A (en) | 2020-06-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111267095B (en) | Mechanical arm grabbing control method based on binocular vision | |
CN108555908B (en) | Stacked workpiece posture recognition and pickup method based on RGBD camera | |
CN107767423B (en) | mechanical arm target positioning and grabbing method based on binocular vision | |
CN112396664B (en) | Monocular camera and three-dimensional laser radar combined calibration and online optimization method | |
CN111775152B (en) | Method and system for guiding mechanical arm to grab scattered stacked workpieces based on three-dimensional measurement | |
CN111721259B (en) | Underwater robot recovery positioning method based on binocular vision | |
CN113096094B (en) | Three-dimensional object surface defect detection method | |
CN111862201A (en) | Deep learning-based spatial non-cooperative target relative pose estimation method | |
CN112907735B (en) | Flexible cable identification and three-dimensional reconstruction method based on point cloud | |
CN111178138B (en) | Distribution network wire operating point detection method and device based on laser point cloud and binocular vision | |
CN111127613B (en) | Image sequence three-dimensional reconstruction method and system based on scanning electron microscope | |
CN113177983B (en) | Fillet weld positioning method based on point cloud geometric features | |
CN114241269B (en) | A collection card vision fuses positioning system for bank bridge automatic control | |
CN111784655A (en) | Underwater robot recovery positioning method | |
US20230368414A1 (en) | Pick and place systems and methods | |
CN112894815A (en) | Method for detecting optimal position and posture for article grabbing by visual servo mechanical arm | |
CN113808201B (en) | Target object detection method and guiding grabbing method | |
CN112767481B (en) | High-precision positioning and mapping method based on visual edge features | |
CN116579955B (en) | New energy battery cell weld reflection point denoising and point cloud complement method and system | |
CN111198563B (en) | Terrain identification method and system for dynamic motion of foot type robot | |
CN115533895B (en) | Two-finger manipulator workpiece grabbing method and system based on vision | |
CN116594351A (en) | Numerical control machining unit system based on machine vision | |
CN116021519A (en) | TOF camera-based picking robot hand-eye calibration method and device | |
CN116664622A (en) | Visual movement control method and device | |
CN112734843B (en) | Monocular 6D pose estimation method based on regular dodecahedron |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |