CN115042175A - Method for adjusting tail end posture of mechanical arm of robot - Google Patents
Method for adjusting tail end posture of mechanical arm of robot Download PDFInfo
- Publication number
- CN115042175A CN115042175A CN202210659413.5A CN202210659413A CN115042175A CN 115042175 A CN115042175 A CN 115042175A CN 202210659413 A CN202210659413 A CN 202210659413A CN 115042175 A CN115042175 A CN 115042175A
- Authority
- CN
- China
- Prior art keywords
- axis
- vertex
- camera
- coordinate system
- mechanical arm
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000012937 correction Methods 0.000 claims abstract description 20
- 238000006243 chemical reaction Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 40
- 230000009466 transformation Effects 0.000 claims description 34
- 230000003287 optical effect Effects 0.000 claims description 21
- 238000012545 processing Methods 0.000 claims description 14
- 239000012636 effector Substances 0.000 claims description 13
- 108700041286 delta Proteins 0.000 claims description 12
- 238000013519 translation Methods 0.000 claims description 12
- 101100518501 Mus musculus Spp1 gene Proteins 0.000 claims description 9
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000003825 pressing Methods 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 5
- 208000037805 labour Diseases 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
- 239000002699 waste material Substances 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/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
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
- B25J19/021—Optical sensing devices
- B25J19/023—Optical sensing devices including video camera means
-
- 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)
- Multimedia (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The invention provides a method for adjusting the tail end posture of a robot mechanical arm, and belongs to the technical field of robot mechanical arm control. The problem that when a mobile robot operates a button of an electrical cabinet, the mechanical arm cannot operate the button accurately due to the fact that the tail end of the mechanical arm of the robot is deviated in posture caused by positioning errors of the mobile robot is mainly solved. The adjusting method comprises the following steps: the robot moves in front of an operation panel of a target electrical cabinet, drives a mechanical arm to drive a camera, shoots a picture of a correction plate, recognizes to obtain the position of a positioning circle, obtains coordinate information of three vertexes according to the position relation of the three vertexes and the positioning circle, calculates the inclination angles of the three directions, then obtains a target posture of the tail end of the mechanical arm under a base coordinate system according to coordinate conversion, and drives the mechanical arm to enable the tail end to reach the target posture. The target posture of the tail end of the mechanical arm is obtained based on the visual information of the depth camera, and the posture of the tail end of the mechanical arm of the robot is adjusted.
Description
Technical Field
The invention belongs to the technical field of robot mechanical arm control, and particularly relates to a method for adjusting the tail end posture of a robot mechanical arm.
Background
The immediate generation of robots is coming and robotics is changing the production and lifestyle of humans profoundly. With the development of the related theoretical research of the robot and the hardware technology, various robots are widely applied to the fields of industrial production, storage, security inspection, space exploration and the like, and the robot replaces the work of people on repeated heavy labor or dangerous work, thereby greatly improving the production and living efficiency. However, no matter the mobile robot or the mechanical arm is used, the use scenes of the mobile robot or the mechanical arm are limited, the functions are single, and relatively complex and flexible tasks cannot be performed. The movable mechanical arm is used as a novel robot, and the robot capable of freely moving and the mechanical arm are structurally combined, so that the working space of the movable robot is wide, and the operation space of the mechanical arm is flexible. And after the mobile mechanical arm is provided with environment sensing sensors such as a laser radar and a camera, the capability of acquiring the environment information can be enhanced to a great extent, and more complex operation can be realized.
In recent years, with the rapid development of national power grid systems and high-speed railway systems, the automation degree of electric power and railway systems in China is obviously improved. The transformer substation is a hub for power transmission, and safe and reliable operation of the transformer substation is closely related to national economic construction and safe operation of an electrified railway traffic system. The transformer substation indoor equipment is limited by factors such as small size and narrow movement space of transformer substation indoor equipment, most of the operation of the transformer substation indoor equipment is carried out manually, operators not only waste time and labor, but also face great labor risk, and meanwhile, the transformer substation faces the problems of manpower cost rising, manpower resource shortage and the like. Aiming at the situation, in order to realize unmanned management of the substation, a plurality of substations introduce a mobile mechanical arm to replace an operator to operate equipment in the substation, for example, a command is issued to a button on an electrical cabinet, but in the practical application process, a robot is difficult to accurately identify and position a target button, and a greater safety problem is often caused because the robot cannot accurately complete the operation process.
The Chinese patent document (CN114378822A) entitled "method for adjusting the pose of the tail end of a robot mechanical arm based on vision", which is published in 22.4.2022, adopts a method for adjusting the pose of the tail end of a robot mechanical arm based on vision, and although the method adjusts the target pose of the tail end of the mechanical arm, the method is limited by the adjustment angles of the fourth, fifth and sixth joints of the mechanical arm, the use range is limited, and the method is difficult to adapt to the adjustment of the pose of the tail end of the robot mechanical arm under the condition of a large adjustment angle.
In the operation of the robot, the accuracy of the tail end posture of the mechanical arm directly influences whether the robot can accurately complete the operation. Wherein, still there is following technical problem to the research of robot operation regulator cubicle button technique:
1. the movable trolley reaches the target electrical cabinet operation position, positioning is not accurate, positioning errors exist, and the initial pose of the mechanical arm, the button photographing pose and the electrical cabinet operation panel on the trolley incline to a certain degree, so that the mechanical arm cannot accurately operate the target button.
2. Under the limitation of an electrical cabinet, an electrical cabinet operation panel may not be perpendicular to the ground, and has a certain inclination, and when the mechanical arm reaches the button photographing pose, the vertical plane of the tail end axis of the mechanical arm is not parallel to the electrical cabinet operation panel, so that in the subsequent camera photographing identification and button positioning processes, the mechanical arm operation always has deviation, and the process of pressing a target button cannot be completed more accurately.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a method for adjusting the tail end posture of a mechanical arm of a robot, which can enable the robot to accurately identify and position the button of an electrical cabinet so as to accurately press the button.
The invention aims to realize the purpose, and provides a method for adjusting the tail end attitude of a mechanical arm of a robotA; the correcting plate is rectangular and is adhered above the operation surface of the electrical cabinet, a black right-angled triangle and a black correcting plate positioning circle are arranged on the correcting plate, the circle center of the correcting plate positioning circle is recorded as a point C4, and three vertexes of the right-angled triangle are respectively recorded as a vertex C 1 Vertex C 2 And vertex C 3 Wherein the vertex C 1 Is the vertex at the right angle of the right triangle; after the correction plate is pasted, the vertex C 1 At the upper left corner of the correction plate, vertex C 1 And vertex C 2 The line of (A) is a horizontal line, and the vertex C 1 And vertex C 3 Is connected with the vertex C 1 And vertex C 2 Is perpendicular to the line connecting point C4 and vertex C 1 Is perpendicular to the vertex C 2 And vertex C 3 And the calibration plate positioning circle is positioned at the right lower part of the right triangle; the correction plate positioning circle is used for identifying and positioning the correction plate and determining the positions of three vertexes by a camera;
the robot comprises an AGV trolley, a 6-degree-of-freedom mechanical arm, an end effector and a depth camera, wherein the 6-degree-of-freedom mechanical arm comprises 6 rotary joints and a mechanical arm base, the 6 rotary joints sequentially comprise a first rotary joint, a second rotary joint, a third rotary joint, a fourth rotary joint, a fifth rotary joint and a sixth rotary joint from the mechanical arm base, and the 6 rotary joints sequentially and respectively rotate around a first joint shaft, a second joint shaft, a third joint shaft, a fourth joint shaft, a fifth joint shaft and a sixth joint shaft, wherein the fourth joint shaft and the fifth joint shaft are mutually vertical, and the fifth joint shaft and the sixth joint shaft are mutually vertical; the end effector and the depth camera are both mounted at the tail end of the 6-degree-of-freedom mechanical arm, the optical axis of the depth camera is parallel to the sixth joint axis, and the depth camera moves along with the 6-degree-of-freedom mechanical arm;
the adjusting method comprises the following steps:
step 1, defining a pixel coordinate system, an image coordinate system, a camera coordinate system, an electrical cabinet operating surface coordinate system, a mechanical arm tail end coordinate system and a mechanical arm base coordinate system, and calibrating a camera by using a depth camera;
the pixel coordinate system is a rectangular coordinate system which is established by taking the upper left corner of the image as an origin and takes the pixel as a unit, the rectangular coordinate system comprises a pixel U axis and a pixel V axis which are perpendicular to each other, the abscissa on the pixel U axis is the number of columns of the pixel in the image, the direction is parallel to the right of the image plane, the ordinate on the pixel V axis is the number of rows of the pixel in the image, and the direction is perpendicular to the pixel U axis and downward;
the image coordinate system is established by taking the intersection point of the camera optical axis and the image plane as an origin, and comprises an image X axis and an image Y axis which are perpendicular to each other, wherein the image X axis is parallel to the pixel U axis and has the same direction, and the image Y axis is parallel to the pixel V axis and has the same direction;
the camera coordinate system is a coordinate system established by taking a camera optical center as an origin and comprises a camera Xc axis, a camera Yc axis and a camera Zc axis, wherein the camera Zc axis is a camera optical axis and a photographing direction is a positive direction, the camera Xc axis is parallel to a pixel U axis and has the same direction, and the camera Yc axis is parallel to a pixel V axis and has the same direction;
the coordinate system of the operation surface of the electrical cabinet is a coordinate system established by taking a point C4 as an original point and comprises an operation surface Xt axis, an operation surface Yt axis and an operation surface Zt axis, wherein the operation surface Zt axis is parallel to the normal of the operation surface, the direction of the operation surface Zt axis points to the inside of the operation surface from the outside of the operation surface, and the operation surface Yt axis and the top point C of the operation surface Zt axis 1 And vertex C 3 The right-angle sides are parallel and the direction is a vertex C 1 Point to vertex C 3 The Xt axis and the vertex C of the plane of operation 1 And vertex C 2 The right-angle sides are parallel and the direction is a vertex C 1 Point to vertex C 2 ；
The coordinate system established by the mechanical arm tail end coordinate system with the six centers of the rotary joints as the original points comprises a mechanical arm tail end Xe axis, a mechanical arm tail end Ye axis and a mechanical arm tail end Ze axis, wherein the mechanical arm tail end Ze axis is a sixth joint axis, namely the mechanical arm tail end Ze axis is parallel to the camera optical axis and has the same direction, the mechanical arm tail end Xe axis is parallel to the camera Xc axis and has the same direction, and the mechanical arm tail end Ye axis is parallel to the camera Yc axis and has the same direction;
the coordinate system of the mechanical arm base is established by taking the central point of the mechanical arm base as an original point and comprises a mechanical arm base X b Shaft and mechanical arm base Y b Shaft and arm base Z b A shaft, wherein the mechanical arm base Z b The axis is parallel to the first joint axis, and the mechanical arm base X b The axis takes the front of the AGV car as the positive direction, and the mechanical arm base Y b The axes are determined by the coordinate system right hand rule;
the camera calibration is carried out on the depth camera by using a camera calibration algorithm, the calibrated result is an internal reference matrix M of the depth camera, and the expression is as follows:
in the formula,
f x is normalized focal length, f, on the X-axis of the image in the image coordinate system x ＝f/d x Where f is the focal length of the depth camera in mm, d x The physical size of each pixel in the pixel coordinate system on the X axis of the image is in mm/pixel;
f y is normalized focal length, f, on the Y-axis of the image in the image coordinate system y ＝f/d y ，d y The physical size of each pixel in the pixel coordinate system on the Y axis of the image is in mm/pixel;
u 0 is the abscissa, v, of the origin of the image coordinate system in the pixel coordinate system 0 Is the ordinate of the origin of the image coordinate system in the pixel coordinate system, i.e. the coordinate of the origin of the image coordinate system in the pixel coordinate system is (u) 0 ，v 0 )；
Step 2, the robot moves to the front of a target electrical cabinet operation panel, the 6-degree-of-freedom mechanical arm is driven to enable the depth camera to reach the shooting position of the correction plate for shooting, then the shot image is subjected to image processing, and a vertex C is obtained 1 Vertex C 2 And vertex C 3 The pixel coordinates and depth values of (a) are: vertex C 1 Pixel coordinate (u) 1 ，v 1 ) And depth value d 1 Vertex C 2 Pixel coordinate (u) 2 ，v 2 ) And depth value d 2 Vertex C 3 Pixel coordinate (u) 3 ，v 3 ) And depth value d 3 ；
Step 3, respectively calculating a vertex C according to the internal reference matrix M calibrated in the step 1 and a vertex coordinate transformation formula I 1 Camera coordinates (x) in camera coordinate system c1 ，y c1 ，z c1 ) Vertex C 2 Camera coordinates (x) in camera coordinate system c2 ，y c2 ，z c2 ) Vertex C 3 Camera coordinates (x) in camera coordinate system c3 ，y c3 ，z c3 ) The expression of the vertex coordinate transformation formula one is as follows:
wherein Z is the depth value measured by the vertex under the camera coordinate system, namely the distance between the vertex and the depth camera plane, u is the abscissa of the vertex under the pixel coordinate system, v is the ordinate of the vertex under the pixel coordinate system, (x) c ，y c ，z c ) The camera coordinates of the vertex under the camera coordinate system are shown, and the depth camera plane is a plane formed by an Xc axis of the camera and an Yc axis of the camera;
step 4, solving the inclination angle between the camera Xc axis and the operation plane Xt axis, the inclination angle between the camera Yc axis and the operation plane Yt axis, and the inclination angle between the camera ZC axis and the operation plane Zt axis of the depth camera according to the coordinate relation of the vertex C1, the vertex C2 and the vertex C3, wherein the solving process is as follows:
step 4.1, making a straight line parallel to the Xc axis of the camera through the vertex C1, making a perpendicular line of the straight line through the vertex C2, and marking the intersection point as an intersection point Q 1 Connecting the vertex C1, the vertex C2 and the intersection point Q 1 Form a right triangle, angle Q 1 C 1 C 2 Namely, the inclination angle between the camera Xc axis of the depth camera and the operation plane Xt axis, and the expression is:
step 4.2, along the direction of the depth camera optical axis, project vertex C3 to over-vertex C1 and flatOn a plane running through the Xc-Yc axis of the camera, denoted as point Q 2 Connecting the vertex C3, the vertex C1 and the point Q 2 Form a right triangle, angle Q 2 C 1 C 3 Namely, the inclination angle between the camera Yc axis of the depth camera and the operation plane Yt axis, and the expression is:
step 4.3, along the direction of the depth camera optical axis, project vertex C1 onto a plane passing through vertex C2 and parallel to the camera Xc-camera Yc axis, denoted as point Q 3 Connecting the vertex C1, the vertex C2 and the point Q 3 Form a right triangle C 1 Q 3 C 2 ，∠Q 3 C 2 C 1 Namely, the inclination angle between the camera Zc axis of the depth camera and the operation plane Zt axis, and its expression is:
step 5, defining a first rotation matrix
The first rotation matrix
The method is used for describing the current posture of the coordinate system of the operation surface of the electric appliance cabinet relative to the coordinate system of the tail end of the mechanical arm, and the expression is as follows:
in the formula,
step 6, defining a third rotation matrix
The third rotation matrix
The method is used for describing the current posture of the electrical cabinet operating surface coordinate system relative to the mechanical arm base coordinate system, and the conversion formula is as follows:
in the formula,
a second rotation matrix set for the 6-degree-of-freedom robot arm, describing the current pose of the robot arm end coordinate system relative to the robot arm base coordinate system, r 11 Is the component of the projection of the Xt axis of the operation surface on the Xb axis of the mechanical arm base 21 Is the component of the projection of the Xt axis of the operation surface on the Yb axis of the robot base, r 31 Is the component of the projection of the Xt axis of the operating surface on the Zb axis of the arm base, r 12 Is a component of projection of the operation plane Yt axis on the robot arm base Xb axis, r 22 Is the component of the projection of the operation surface Yt axis on the robot base Yb axis, r 32 Is the component of the projection of the operation plane Yt axis on the axis of the arm base Zb, r 13 Is the component of the projection of the Zt axis of the operating plane on the Xb axis of the mechanical arm base 23 Is the component of the projection of the Zt axis of the operating surface on the Yb axis of the arm base, r 33 Is the projection component of the operation surface Zt axis on the axis of the mechanical arm base Zb; defining a first Euler angle
The first Euler angle
Used for describing the current attitude, the first Euler angle, of the electrical cabinet operating surface coordinate system relative to the mechanical arm base coordinate system
The arctan expression of (c) is as follows:
when in use
When the temperature of the water is higher than the set temperature,
when in use
When the temperature of the water is higher than the set temperature,
when in use
In the formula, Atan2() is a bivariate arctangent function;
defining a second Euler angle
The second Euler angle
For describing the current attitude of the robot arm end coordinate system relative to the robot arm base coordinate system, the second Euler angle
The calculation formula of (A) is as follows:
and 7, driving the 6-degree-of-freedom mechanical arm to enable the tail end coordinate system of the mechanical arm to move to a second Euler angle
In the described attitude;
step 8, obtaining the vertex C again according to the method of the step 2 1 Vertex C 2 Vertex C 3 And respectively recording as the adjusted vertex C 1 Pixel coordinate (u) 1n ，v 1n ) And depth value d 1n The adjusted vertex C 2 Pixel coordinate (u) 2n ，v 2n ) And depth value d 2n The adjusted vertex C 3 Pixel coordinate (u) 3n ，v 3n ) And depth value d 3n Wherein n is the adjusting times, and n is more than or equal to 1;
a first difference δ 1, a second difference δ 2, a third difference δ 3, a fourth difference η 1 and a fifth difference η 2 are defined, where δ 1 ═ d 1n -d 2n |，δ2＝|d 1n -d 3n |，δ3＝|d 2n -d 3n |，η1＝|v 1n -v 2n |，η2＝|u 1n -u 3n L, |; giving a first preset value delta and a second preset value eta, and judging as follows:
if delta 1 is larger than delta, delta 2 is larger than delta, delta 3 is larger than delta, eta 1 is larger than eta, and eta 2 is larger than eta, the adjustment of the tail end posture of the mechanical arm with 6 degrees of freedom is finished, and the step 9 is entered;
if delta 1 is less than delta, delta 2 is less than delta, delta 3 is less than delta, eta 1 is less than eta and eta 2 is less than eta, returning to the step 2 for the next round of adjustment;
step 9, after the tail end of the 6-freedom-degree mechanical arm finishes posture adjustment, the 6-freedom-degree mechanical arm is driven to enable the depth camera to reach the button photographing pose to photograph, then the photographed image is subjected to image processing, and the pixel coordinate (u) of the point A is obtained a ，v a ) And depth value d a ；
Step 10, firstly, the vertex coordinate transformation formula I in step 3 is used to convert the pixel coordinate (u) of the point A obtained in step 9 a ，v a ) Converted into camera coordinates (x) of point A in the camera coordinate system ca 、y ca 、z ca ) (ii) a Then, the camera coordinates (x) of the point A are transformed by using a vertex coordinate transformation formula II ca 、y ca 、z ca ) Converting the coordinate into the coordinate (x) of the mechanical arm base under the coordinate system of the mechanical arm base ba 、y ba 、z ba ) The expression of the vertex coordinate transformation formula II is as follows:
wherein,
is a camera external reference matrix calibrated by the hand eye of the mechanical arm,
is a homogeneous transformation matrix obtained by a mechanical arm system;
step 11, driving the 6-degree-of-freedom mechanical arm to enable the end effector to reach (x) ba 、y ba 、z ba ) The pose is followed by pressing a button by the end effector to finish the operation;
and 12, 6, restoring the mechanical arm with the degree of freedom 6 to the pose when the mechanical arm is not operated, and waiting for the instruction of the next operation.
Preferably, the image processing of step 2 includes image graying and hough circle transformation, and the image processing of step 7 includes format conversion and hough circle transformation.
Preferably, the camera extrinsic parameter matrix
The pose transformation method is used for describing the pose transformation of the tail end coordinate system of the mechanical arm relative to the camera coordinate system, and the expression is as follows:
in the formula,
is a 3 x 3 rotation matrix describing the rotation of the robot arm tip coordinate system with respect to the camera coordinate system,
is a translation vector of 3 multiplied by 1, which is used for describing the coordinate translation of the mechanical arm end coordinate system relative to the camera coordinate system;
the homogeneous transformation matrix
The pose transformation for describing the coordinate system of the tail end of the mechanical arm relative to the coordinate system of the base of the mechanical arm is expressed as follows:
in the formula,
is a 3 x 3 rotation matrix for describing the rotation of the robot arm tip coordinate system with respect to the robot arm base coordinate system,
is a 3 x 1 translation vector describing the translation of the robot arm tip coordinate system relative to the robot arm base coordinate system.
Compared with the prior art, the invention has the following beneficial effects;
1. the invention adopts the method for adjusting the tail end gesture of the robot mechanical arm, is used for the robot mechanical arm to operate the button on the electrical cabinet, and solves the problem that the tail end gesture of the robot mechanical arm is influenced by AGV trolley positioning error and electrical cabinet working limitation, so that the robot mechanical arm can more accurately operate the button of the electrical cabinet.
2. The invention adopts the method for adjusting the tail end posture of the robot mechanical arm, has simple and quick correction process and easy implementation, and meets the requirement of the robot mechanical arm on operating the button of the electrical cabinet.
Drawings
FIG. 1 is a block diagram of a robotic system of the present invention;
FIG. 2 is a front view of an electrical cabinet operating surface according to the present invention;
FIG. 3 is a flow chart of a tuning method of the present invention;
FIG. 4 is an inclination angle Q between the Xc axis of the depth camera and the Xt axis of the operation surface in the invention 1 C 1 C 2 A schematic diagram of (a);
FIG. 5 is an inclination angle Q between the Yc axis of the depth camera and the Yt axis of the operation surface in the invention 2 C 1 C 3 A schematic diagram of (a);
FIG. 6 is an inclination angle Q between the Zc axis of the depth camera and the Zt axis of the operation surface in the invention 3 C 2 C 1 A schematic diagram of (a);
FIG. 7 is a calibration plate image taken after calibration of the end of the robot arm of the present invention;
FIG. 8 is a simplified flow chart of the tuning method of the present invention.
Detailed Description
The embodiments of the present invention will be further described with reference to the accompanying drawings.
Fig. 1 is a structural diagram of a robot system according to the present invention, and fig. 2 is a front view of an operation surface of an electrical cabinet according to the present invention. As can be seen from fig. 1 and 2, the system to which the adjustment method relates includes a robot, an electrical cabinet, and a correction plate. The electric cabinet is characterized in that a button is arranged on the operation surface of the electric cabinet, and the circle center of the button is marked as a point A. The correcting plate is rectangular and is adhered above the operation surface of the electrical cabinet, a black right-angled triangle and a black correcting plate positioning circle are arranged on the correcting plate, the circle center of the correcting plate positioning circle is recorded as a point C4, and three vertexes of the right-angled triangle are respectively recorded as a vertex C 1 Vertex C 2 And vertex C 3 Wherein the vertex C 1 Is the vertex at the right angle of the right triangle; after the correction plate is pasted, the vertex C 1 At the upper left corner of the correction plate, vertex C 1 And vertex C 2 Is a horizontal line, and has a vertex C 1 And vertex C 3 Is connected with the vertex C 1 And vertex C 2 Is perpendicular to the line connecting point C4 and vertex C 1 Is perpendicular to the vertex C 2 And vertex C 3 And the calibration plate positioning circle is positioned at the right lower part of the right triangle; the correction plate positioning circle is used for identifying and positioning the correction plate and determining three vertexes of the correction plateThe position of (a).
The robot includes AGV dolly, 6 degrees of freedom arms, end effector and degree of depth camera, 6 degrees of freedom arms include 6 revolute joint and arm base, 6 revolute joint is rotary joint one, rotary joint two, rotary joint three, rotary joint four, rotary joint five, rotary joint six by the arm base in proper order, and 6 revolute joint is in proper order respectively around first joint axle, second joint axle, third joint axle, fourth joint axle, fifth joint axle, the rotation of sixth joint axle, wherein, fourth joint axle and fifth joint axle mutually perpendicular, fifth joint axle and sixth joint axle mutually perpendicular. The end effector and the depth camera are both mounted at the tail end of the 6-degree-of-freedom mechanical arm, the optical axis of the depth camera is parallel to the sixth joint axis, and the depth camera moves along with the 6-degree-of-freedom mechanical arm.
In the present adjustment method, the end effector is used to press a button.
Fig. 3 is a flow chart of the adjusting method of the present invention, and fig. 8 is a flow chart of the adjusting method of the present invention. As can be seen from fig. 3 and 8, the adjusting method of the present invention includes the following steps:
step 1, defining a pixel coordinate system, an image coordinate system, a camera coordinate system, an electrical cabinet operating surface coordinate system, a mechanical arm tail end coordinate system and a mechanical arm base coordinate system, and calibrating a camera by using a depth camera.
The pixel coordinate system is a rectangular coordinate system which is established by taking the upper left corner of the image as an origin and takes the pixel as a unit, the rectangular coordinate system comprises a pixel U axis and a pixel V axis which are perpendicular to each other, the abscissa on the pixel U axis is the number of columns of the pixel in the image, the direction is parallel to the right of the image plane, the ordinate on the pixel V axis is the number of rows of the pixel in the image, and the direction is perpendicular to the pixel U axis and downward;
the image coordinate system is established by taking the intersection point of the camera optical axis and the image plane as an origin, and comprises an image X axis and an image Y axis which are perpendicular to each other, wherein the image X axis is parallel to the pixel U axis and has the same direction, and the image Y axis is parallel to the pixel V axis and has the same direction;
the camera coordinate system is a coordinate system established by taking a camera optical center as an origin and comprises a camera Xc axis, a camera Yc axis and a camera Zc axis, wherein the camera Zc axis is a camera optical axis and a photographing direction is a positive direction, the camera Xc axis is parallel to a pixel U axis and has the same direction, the camera Yc axis is parallel to a pixel V axis and has the same direction, and the camera Zc axis can be vertical to an electrical cabinet operating surface or not;
the coordinate system of the operation surface of the electric cabinet is a coordinate system established by taking a point C4 as an original point and comprises an operation surface Xt axis, an operation surface Yt axis and an operation surface Zt axis, wherein the operation surface Zt axis is parallel to the normal of the operation surface, the direction of the operation surface Zt axis points to the inside of the operation surface from the outside of the operation surface, and the operation surface Yt axis and the vertex C of the operation surface Yt axis 1 And vertex C 3 The right-angle sides are parallel and the direction is a vertex C 1 Point to vertex C 3 The Xt axis and the vertex C of the plane of operation 1 And vertex C 2 The right-angle sides are parallel and the direction is a vertex C 1 Point to vertex C 2 ；
The coordinate system established by the mechanical arm tail end coordinate system with the six centers of the rotary joints as the original points comprises a mechanical arm tail end Xe axis, a mechanical arm tail end Ye axis and a mechanical arm tail end Ze axis, wherein the mechanical arm tail end Ze axis is a sixth joint axis, namely the mechanical arm tail end Ze axis is parallel to the camera optical axis and has the same direction, the mechanical arm tail end Xe axis is parallel to the camera Xc axis and has the same direction, and the mechanical arm tail end Ye axis is parallel to the camera Yc axis and has the same direction;
the coordinate system of the mechanical arm base is established by taking the central point of the mechanical arm base as an original point and comprises a mechanical arm base X b Shaft and mechanical arm base Y b Shaft and arm base Z b A shaft, wherein the mechanical arm base Z b The axis is parallel to the first joint axis, and the mechanical arm base X b The axis takes the positive direction of the AGV trolley and the mechanical arm base Y b The axes are determined by the coordinate system right hand rule.
The camera calibration is carried out on the depth camera by using a camera calibration algorithm, the calibrated result is an internal reference matrix M of the depth camera, and the expression is as follows:
in the formula,
f x is a normalized focal length, f, on the X-axis of the image in the image coordinate system x ＝f/d x Where f is the focal length of the depth camera in mm, d x The physical size of each pixel in the pixel coordinate system on the X axis of the image is in mm/pixel;
f y is normalized focal length, f, on the Y-axis of the image in the image coordinate system y ＝f/d y ，d y The physical size of each pixel in the pixel coordinate system on the Y axis of the image is in mm/pixel;
u 0 is the abscissa, v, of the origin of the image coordinate system in the pixel coordinate system 0 Is the ordinate of the origin of the image coordinate system in the pixel coordinate system, i.e. the coordinate of the origin of the image coordinate system in the pixel coordinate system is (u) 0 ，v 0 )。
Step 2, the robot moves to the front of a target electrical cabinet operation panel, the 6-degree-of-freedom mechanical arm is driven to enable the depth camera to reach the shooting position of the correction plate for shooting, then the shot image is subjected to image processing, and a vertex C is obtained 1 Vertex C 2 And vertex C 3 The pixel coordinates and depth values of (a) are: vertex C 1 Pixel coordinate (u) 1 ，v 1 ) And depth value d 1 Vertex C 2 Pixel coordinate (u) 2 ，v 2 ) And depth value d 2 Vertex C 3 Pixel coordinate (u) 3 ，v 3 ) And depth value d 3 。
In this embodiment, the image processing includes image graying and hough circle transformation, and the image processing in step 7 includes format conversion and hough circle transformation.
The image graying refers to graying the original image by using an image processing tool so as to make black and white differentiation more obvious; the format conversion refers to converting the image in the RGB format into the image in the HSV format by using an image processing tool; the Hough circle transformation is used for detecting circles existing in the image and obtaining the pixel coordinates of the circle center of the circle.
In the step (3), the step (B),respectively calculating a vertex C according to the internal reference matrix M calibrated in the step 1 and a vertex coordinate transformation formula I 1 Camera coordinates (x) in camera coordinate system c1 ，y c1 ，z c1 ) Vertex C 2 Camera coordinates (x) in camera coordinate system c2 ，y c2 ，z c2 ) Vertex C 3 Camera coordinates (x) in camera coordinate system c3 ，y c3 ，z c3 ) The expression of the vertex coordinate transformation formula one is as follows:
wherein Z is the depth value measured by the vertex under the camera coordinate system, namely the distance between the vertex and the depth camera plane, u is the abscissa of the vertex under the pixel coordinate system, v is the ordinate of the vertex under the pixel coordinate system, (x) c ，y c ，z c ) The camera coordinates of the vertex in the camera coordinate system are shown, and the depth camera plane is a plane formed by the camera Xc axis and the camera Yc axis.
Step 4, solving the inclination angle between the camera Xc axis and the operation plane Xt axis, the inclination angle between the camera Yc axis and the operation plane Yt axis, and the inclination angle between the camera ZC axis and the operation plane Zt axis of the depth camera according to the coordinate relation of the vertex C1, the vertex C2 and the vertex C3, wherein the solving process is as follows:
step 4.1, making a straight line parallel to the Xc axis of the camera through the vertex C1, making a perpendicular line of the straight line through the vertex C2, and marking the intersection point as an intersection point Q 1 Connecting the vertex C1, the vertex C2 and the intersection point Q 1 Form a right triangle, angle Q 1 C 1 C 2 Namely, the inclination angle between the camera Xc axis of the depth camera and the operation plane Xt axis, and the expression is:
step 4.2, along the direction of the depth camera optical axis, project vertex C3 onto a plane passing through vertex C1 and parallel to the camera Xc-camera Yc axisIs recorded as a point Q 2 Connecting the vertex C3, the vertex C1 and the point Q 2 Form a right triangle, angle Q 2 C 1 C 3 Namely, the inclination angle between the camera Yc axis of the depth camera and the operation plane Yt axis, and the expression is:
step 4.3, along the direction of the depth camera optical axis, project vertex C1 onto a plane passing through vertex C2 and parallel to the camera Xc-camera Yc axis, denoted as point Q 3 Connecting the vertex C1, the vertex C2 and the point Q 3 Form a right triangle C 1 Q 3 C 2 ，∠Q 3 C 2 C 1 Namely, the inclination angle between the camera Zc axis of the depth camera and the operation plane Zt axis, and its expression is:
fig. 4, fig. 5 and fig. 6 show three inclination angles Q 1 C 1 C 2 、∠Q 2 C 1 C 3 And < Q 3 C 2 C 1 Schematic illustration of (a).
Step 5, defining a first rotation matrix
The first rotation matrix
The method is used for describing the current posture of the coordinate system of the operation surface of the electric appliance cabinet relative to the coordinate system of the tail end of the mechanical arm, and the expression is as follows:
in the formula,
step 6, defining a third rotation matrix
The third rotation matrix
The method is used for describing the current posture of the coordinate system of the operation surface of the electric cabinet relative to the coordinate system of the mechanical arm base, and the conversion formula is as follows:
in the formula,
a second rotation matrix set for the 6-degree-of-freedom robot arm, describing the current pose of the robot arm end coordinate system relative to the robot arm base coordinate system, r 11 Is the projected component of the Xt axis of the operation surface on the Xb axis of the mechanical arm base, r 21 Is the component of the projection of the Xt axis of the operation surface on the Yb axis of the robot base, r 31 Is the component of the projection of the Xt axis of the operating surface on the Zb axis of the arm base, r 12 Is a component of projection of the operation plane Yt axis on the robot arm base Xb axis, r 22 Is the component of the projection of the operation surface Yt axis on the robot base Yb axis, r 32 Is the component of the projection of the operation plane Yt axis on the axis of the arm base Zb, r 13 Is the component of the projection of the Zt axis of the operating plane on the Xb axis of the mechanical arm base 23 Is the component of the projection of the Zt axis of the operation surface on the Yb axis of the mechanical arm base, r 33 Is the projection component of the operation surface Zt axis on the axis of the mechanical arm base Zb;
defining a first Euler angle
The first Euler angle
For describing electric appliancesCurrent attitude, first Euler angle, of the cabinet operating surface coordinate system relative to the robot arm base coordinate system
The arctan expression of (c) is as follows:
when in use
When the temperature of the water is higher than the set temperature,
when the temperature is higher than the set temperature
When the temperature of the water is higher than the set temperature,
when in use
In the formula, Atan2() is a bivariate arctangent function.
Defining a second Euler angle
The second Euler angle
For describing the current attitude of the robot arm end coordinate system relative to the robot arm base coordinate system, the second Euler angle
The calculation formula of (A) is as follows:
and 7, driving the 6-degree-of-freedom mechanical arm to enable the tail end coordinate system of the mechanical arm to move to a second Euler angle
In the described attitude.
Step 2-step 7 are a one-time adjustment process. FIG. 7 is a calibration board taken after the end of the robot arm is calibrated.
Step 8, obtaining the vertex C again according to the method of the step 2 1 Vertex C 2 Vertex C 3 And respectively recording as the adjusted vertex C 1 Pixel coordinate (u) 1n ，v 1n ) And depth value d 1n The adjusted vertex C 2 Pixel coordinate (u) 2n ，v 2n ) And depth value d 2n And the adjusted vertex C 3 Pixel coordinate (u) 3n ，v 3n ) And depth value d 3n Wherein n is the adjustment times, and n is more than or equal to 1.
A first difference δ 1, a second difference δ 2, a third difference δ 3, a fourth difference η 1 and a fifth difference η 2 are defined, where δ 1 ═ d 1n -d 2n |，δ2＝|d 1n -d 3n |，δ3＝|d 2n -d 3n |，η1＝|v 1n -v 2n |，η2＝|u 1n -u 3n L, |; giving a first preset value delta and a second preset value eta, and judging as follows:
if delta 1 is larger than delta, delta 2 is larger than delta, delta 3 is larger than delta, eta 1 is larger than eta, and eta 2 is larger than eta, the adjustment of the tail end posture of the mechanical arm with 6 degrees of freedom is finished, and the step 9 is entered;
if delta 1 is less than delta, delta 2 is less than delta, delta 3 is less than delta, eta 1 is less than eta, eta 2 is less than eta, the step 2 is returned to, and the next round of adjustment is carried out.
In the present embodiment, the first preset value δ is 1mm, and the second preset value η is 2 pixels.
Step 9, after the tail end of the 6-freedom-degree mechanical arm finishes posture adjustment, the 6-freedom-degree mechanical arm is driven to enable the depth camera to reach the button photographing pose to photograph, then the photographed image is subjected to image processing, and the pixel coordinate (u) of the point A is obtained a ，v a ) And depth value d a 。
Step 10, firstly, the vertex coordinate transformation formula I in step 3 is used to convert the pixel coordinate (u) of the point A obtained in step 9 a ，v a ) Converted into camera coordinates (x) of point A in the camera coordinate system ca 、y ca 、z ca ) (ii) a Then, the camera coordinates (x) of the point A are transformed by using a vertex coordinate transformation formula II ca 、y ca 、z ca ) Converting the coordinate into the coordinate (x) of the mechanical arm base under the coordinate system of the mechanical arm base ba 、y ba 、z ba ) The expression of the vertex coordinate transformation formula two is as follows:
wherein,
is a camera external reference matrix calibrated by the hand eye of the mechanical arm,
is a homogeneous transformation matrix derived from a robotic arm system.
In this embodiment, the camera extrinsic parameter matrix
The pose transformation for describing the terminal coordinate system of the mechanical arm relative to the camera coordinate system is expressed as follows:
in the formula,
is a 3 x 3 rotation matrix describing the rotation of the robot arm tip coordinate system with respect to the camera coordinate system,
is a translation vector of 3 multiplied by 1, which is used for describing the coordinate translation of the mechanical arm end coordinate system relative to the camera coordinate system;
the homogeneous transformation matrix
The pose transformation for describing the coordinate system of the tail end of the mechanical arm relative to the coordinate system of the base of the mechanical arm is expressed as follows:
in the formula,
is a 3 x 3 rotation matrix for describing the rotation of the robot arm tip coordinate system with respect to the robot arm base coordinate system,
is a 3 x 1 translation vector describing the translation of the robot arm tip coordinate system relative to the robot arm base coordinate system.
Step 11, driving the 6-degree-of-freedom mechanical arm to enable the end effector to reach (x) ba 、y ba 、z ba ) And (5) the pose, and then the end effector presses down the button to finish the operation.
And 12, 6, restoring the mechanical arm with the degree of freedom 6 to the pose when the mechanical arm is not operated, and waiting for the instruction of the next operation.
Claims (3)
1. A method for adjusting the tail end posture of a mechanical arm of a robot is characterized in that a system related to the method for adjusting the tail end posture of the mechanical arm of the robot comprises the robot, an electrical cabinet and a correction plate, wherein a button is arranged on an operation surface of the electrical cabinet, and the circle center of the button is marked as a point A; the correcting plate is rectangular and is adhered above the operation surface of the electrical cabinet, a black right-angled triangle and a black correcting plate positioning circle are arranged on the correcting plate, the circle center of the correcting plate positioning circle is recorded as a point C4, and three vertexes of the right-angled triangle are respectivelyIs denoted as vertex C 1 Vertex C 2 And vertex C 3 Wherein the vertex C 1 Is the vertex at the right angle of the right triangle; after the correction plate is pasted, the vertex C 1 At the upper left corner of the correction plate, vertex C 1 And vertex C 2 Is a horizontal line, and has a vertex C 1 And vertex C 3 Is connected with the vertex C 1 And vertex C 2 Is vertical, point C4 and vertex C 1 Is perpendicular to the vertex C 2 And vertex C 3 And the calibration plate positioning circle is positioned at the right lower part of the right triangle; the correction plate positioning circle is used for identifying and positioning the correction plate and determining the positions of three vertexes by a camera;
the robot comprises an AGV trolley, a 6-degree-of-freedom mechanical arm, an end effector and a depth camera, wherein the 6-degree-of-freedom mechanical arm comprises 6 rotary joints and a mechanical arm base, the 6 rotary joints sequentially comprise a first rotary joint, a second rotary joint, a third rotary joint, a fourth rotary joint, a fifth rotary joint and a sixth rotary joint from the mechanical arm base, and the 6 rotary joints sequentially and respectively rotate around a first joint shaft, a second joint shaft, a third joint shaft, a fourth joint shaft, a fifth joint shaft and a sixth joint shaft, wherein the fourth joint shaft and the fifth joint shaft are perpendicular to each other, and the fifth joint shaft and the sixth joint shaft are perpendicular to each other; the end effector and the depth camera are both mounted at the tail end of the 6-degree-of-freedom mechanical arm, the optical axis of the depth camera is parallel to the sixth joint axis, and the depth camera moves along with the 6-degree-of-freedom mechanical arm;
the adjusting method comprises the following steps:
step 1, defining a pixel coordinate system, an image coordinate system, a camera coordinate system, an electrical cabinet operating surface coordinate system, a mechanical arm tail end coordinate system and a mechanical arm base coordinate system, and calibrating a camera by using a depth camera;
the pixel coordinate system is a rectangular coordinate system which is established by taking the upper left corner of the image as an origin and takes the pixel as a unit, the rectangular coordinate system comprises a pixel U axis and a pixel V axis which are perpendicular to each other, the abscissa on the pixel U axis is the number of columns of the pixel in the image, the direction is parallel to the right of the image plane, the ordinate on the pixel V axis is the number of rows of the pixel in the image, and the direction is perpendicular to the pixel U axis and downward;
the image coordinate system is established by taking the intersection point of the camera optical axis and the image plane as an origin, and comprises an image X axis and an image Y axis which are perpendicular to each other, wherein the image X axis is parallel to the pixel U axis and has the same direction, and the image Y axis is parallel to the pixel V axis and has the same direction;
the camera coordinate system is a coordinate system established by taking a camera optical center as an origin and comprises a camera Xc axis, a camera Yc axis and a camera Zc axis, wherein the camera Zc axis is a camera optical axis and a photographing direction is a positive direction, the camera Xc axis is parallel to a pixel U axis and has the same direction, and the camera Yc axis is parallel to a pixel V axis and has the same direction;
the coordinate system of the operation surface of the electric cabinet is a coordinate system established by taking a point C4 as an original point and comprises an operation surface Xt axis, an operation surface Yt axis and an operation surface Zt axis, wherein the operation surface Zt axis is parallel to the normal of the operation surface, the direction of the operation surface Zt axis points to the inside of the operation surface from the outside of the operation surface, and the operation surface Yt axis and the vertex C of the operation surface Yt axis 1 And vertex C 3 The right-angle sides are parallel and the direction is a vertex C 1 Point to vertex C 3 The Xt axis and the vertex C of the plane of operation 1 And vertex C 2 The right-angle sides are parallel and the direction is a vertex C 1 Point to vertex C 2 ；
The coordinate system established by the mechanical arm tail end coordinate system with the six centers of the rotary joints as the original points comprises a mechanical arm tail end Xe axis, a mechanical arm tail end Ye axis and a mechanical arm tail end Ze axis, wherein the mechanical arm tail end Ze axis is a sixth joint axis, namely the mechanical arm tail end Ze axis is parallel to the camera optical axis and has the same direction, the mechanical arm tail end Xe axis is parallel to the camera Xc axis and has the same direction, and the mechanical arm tail end Ye axis is parallel to the camera Yc axis and has the same direction;
the coordinate system of the mechanical arm base is established by taking the central point of the mechanical arm base as an original point and comprises a mechanical arm base X b Shaft and mechanical arm base Y b Shaft and arm base Z b A shaft, wherein the mechanical arm base Z b The axis is parallel to the first joint axis, and the mechanical arm base X b The axis takes the positive direction of the AGV trolley and the mechanical arm base Y b The axes are determined by the coordinate system right hand rule;
the camera calibration is carried out on the depth camera by using a camera calibration algorithm, the calibrated result is an internal reference matrix M of the depth camera, and the expression is as follows:
in the formula,
f x is normalized focal length, f, on the X-axis of the image in the image coordinate system x ＝f/d x Where f is the focal length of the depth camera in mm, d x The physical size of each pixel in the pixel coordinate system on the X axis of the image is in mm/pixel;
f y is normalized focal length, f, on the Y-axis of the image in the image coordinate system y ＝f/d y ，d y The physical size of each pixel in the pixel coordinate system on the Y axis of the image is in mm/pixel;
u 0 is the abscissa, v, of the origin of the image coordinate system in the pixel coordinate system 0 Is the ordinate of the origin of the image coordinate system in the pixel coordinate system, i.e. the coordinate of the origin of the image coordinate system in the pixel coordinate system is (u) 0 ，v 0 )；
Step 2, the robot moves to the front of a target electrical cabinet operation panel, the 6-degree-of-freedom mechanical arm is driven to enable the depth camera to reach the shooting position of the correction plate for shooting, then the shot image is subjected to image processing, and pixel coordinates and depth values of a vertex C1, a vertex C2 and a vertex C3 are obtained, wherein the pixel coordinates and the depth values are respectively as follows: pixel coordinate (u) of vertex C1 1 ，v 1 ) And depth value d 1 Vertex C 2 Pixel coordinate (u) 2 ，v 2 ) And depth value d 2 Vertex C 3 Pixel coordinate (u) 3 ，v 3 ) And depth value d 3 ；
Step 3, respectively calculating a vertex C according to the internal reference matrix M calibrated in the step 1 and a vertex coordinate transformation formula I 1 Camera coordinates (x) in camera coordinate system c1 ，y c1 ，z c1 ) Vertex C 2 Camera coordinates (x) in camera coordinate system c2 ，y c2 ，z c2 ) Vertex C 3 Camera coordinates (x) in camera coordinate system c3 ，y c3 ，z c3 ) The expression of the vertex coordinate transformation formula one is as follows:
wherein Z is the depth value measured by the vertex under the camera coordinate system, namely the distance between the vertex and the depth camera plane, u is the abscissa of the vertex under the pixel coordinate system, v is the ordinate of the vertex under the pixel coordinate system, (x) c ，y c ，z c ) The camera coordinates of the vertex under the camera coordinate system, and the depth camera plane is a plane formed by a camera Xc axis and a camera Yc axis;
step 4, solving the inclination angle between the camera Xc axis and the operation plane Xt axis, the inclination angle between the camera Yc axis and the operation plane Yt axis, and the inclination angle between the camera ZC axis and the operation plane Zt axis of the depth camera according to the coordinate relation of the vertex C1, the vertex C2 and the vertex C3, wherein the solving process is as follows:
step 4.1, making a straight line parallel to the Xc axis of the camera through the vertex C1, making a perpendicular line of the straight line through the vertex C2, and marking the intersection point as an intersection point Q 1 Connecting the vertex C1, the vertex C2 and the intersection point Q 1 Form a right triangle, angle Q 1 C 1 C 2 Namely, the inclination angle between the camera Xc axis of the depth camera and the operation plane Xt axis, and the expression is:
step 4.2, along the direction of the depth camera optical axis, project vertex C3 onto a plane passing through vertex C1 and parallel to the camera Xc-camera Yc axis, denoted as point Q 2 Connecting the vertex C3, the vertex C1 and the point Q 2 Form a right triangle, angle Q 2 C 1 C 3 Namely, the inclination angle between the camera Yc axis of the depth camera and the operation plane Yt axis, and the expression is:
step 4.3, along the direction of the depth camera optical axis, project vertex C1 onto a plane passing through vertex C2 and parallel to the camera Xc-camera Yc axis, denoted as point Q 3 Connecting the vertex C1, the vertex C2 and the point Q 3 Form a right triangle C 1 Q 3 C 2 ，∠Q 3 C 2 C 1 Namely, the inclination angle between the camera Zc axis of the depth camera and the operation plane Zt axis, and its expression is:
step 5, defining a first rotation matrix
The first rotation matrix
The method is used for describing the current posture of the coordinate system of the operation surface of the electric appliance cabinet relative to the coordinate system of the tail end of the mechanical arm, and the expression is as follows:
in the formula,
step 6, defining a third rotation matrix
The third rotation matrix
The method is used for describing the current posture of the electrical cabinet operating surface coordinate system relative to the mechanical arm base coordinate system, and the conversion formula is as follows:
in the formula,
a second rotation matrix set for the 6-degree-of-freedom robot arm, describing the current pose of the robot arm end coordinate system relative to the robot arm base coordinate system, r 11 Is the component of the projection of the Xt axis of the operation surface on the Xb axis of the mechanical arm base 21 Is the component of the projection of the Xt axis of the operation plane on the Yb axis of the base of the mechanical arm, r 31 Is the component of the projection of the Xt axis of the operating surface on the Zb axis of the arm base, r 12 Is a component of projection of the operation plane Yt axis on the robot arm base Xb axis, r 22 Is the component of the projection of the operation surface Yt axis on the robot base Yb axis, r 32 Is the component of the projection of the operation plane Yt axis on the axis of the arm base Zb, r 13 Is the component of the projection of the Zt axis of the operating plane on the Xb axis of the mechanical arm base 23 Is the component of the projection of the Zt axis of the operating surface on the Yb axis of the arm base, r 33 Is the projection component of the operation surface Zt axis on the axis of the mechanical arm base Zb;
defining a first Euler angle
The first Euler angle
Used for describing the current attitude, the first Euler angle, of the electrical cabinet operating surface coordinate system relative to the mechanical arm base coordinate system
The arctan expression of (c) is as follows:
when in use
When the temperature of the water is higher than the set temperature,
when in use
When the temperature of the water is higher than the set temperature,
when in use
In the formula, Atan2() is a bivariate arctangent function;
defining a second Euler angle
The second Euler angle
For describing the current attitude of the robot arm end coordinate system relative to the robot arm base coordinate system, the second Euler angle
The calculation formula of (A) is as follows:
and 7, driving the 6-degree-of-freedom mechanical arm to enable the tail end coordinate system of the mechanical arm to move to a second Euler angle
In the described attitude;
step 8, obtaining the vertex C again according to the method of the step 2 1 Vertex C 2 Vertex C 3 And respectively recording as the adjusted vertex C 1 Pixel coordinate (u) 1n ，v 1n ) And depth value d 1n The adjusted vertex C 2 Pixel coordinate (u) 2n ，v 2n ) And depth value d 2n The adjusted vertex C 3 Pixel coordinate (u) 3n ，v 3n ) And depth value d 3n Wherein n is the adjusting times, and n is more than or equal to 1;
a first difference δ 1, a second difference δ 2, a third difference δ 3, a fourth difference η 1 and a fifth difference η 2 are defined, where δ 1 ═ d 1n -d 2n |，δ2＝|d 1n -d 3n |，δ3＝|d 2n -d 3n |，η1＝|v 1n -v 2n |，η2＝|u 1n -u 3n L, |; giving a first preset value delta and a second preset value eta, and judging as follows:
if delta 1 is larger than delta, delta 2 is larger than delta, delta 3 is larger than delta, eta 1 is larger than eta, and eta 2 is larger than eta, the adjustment of the tail end posture of the mechanical arm with 6 degrees of freedom is finished, and the step 9 is entered;
if delta 1 is less than delta, delta 2 is less than delta, delta 3 is less than delta, eta 1 is less than eta and eta 2 is less than eta, returning to the step 2 for the next round of adjustment;
step 9, after the tail end of the 6-freedom-degree mechanical arm finishes posture adjustment, the 6-freedom-degree mechanical arm is driven to enable the depth camera to reach the button photographing pose to photograph, then the photographed image is subjected to image processing, and the pixel coordinate (u) of the point A is obtained a ，v a ) And depth value d a ；
Step 10, firstly, the vertex coordinate transformation formula I in step 3 is used to convert the pixel coordinate (u) of the point A obtained in step 9 a ，v a ) Converted into camera coordinates (x) of point A in the camera coordinate system ca 、y ca 、z ca ) (ii) a Then, the camera coordinates (x) of the point A are transformed by using a vertex coordinate transformation formula II ca 、y ca 、z ca ) Convert into mechanical arm baseMechanical arm base coordinate (x) under seat coordinate system ba 、y ba 、z ba ) The expression of the vertex coordinate transformation formula II is as follows:
wherein,
is a camera external reference matrix calibrated by the hand eye of the mechanical arm,
is a homogeneous transformation matrix obtained by a mechanical arm system;
step 11, driving the 6-degree-of-freedom mechanical arm to enable the end effector to reach (x) ba 、y ba 、z ba ) The pose is followed by pressing a button by the end effector to finish the operation;
and 12, 6, restoring the mechanical arm with the degree of freedom 6 to the pose when the mechanical arm is not operated, and waiting for the instruction of the next operation.
2. The method for adjusting the pose of the tail end of the robot manipulator of claim 1, wherein the image processing of the step 2 comprises image graying and Hough circle transformation, and the image processing of the step 7 comprises format conversion and Hough circle transformation.
3. The method of claim 1, wherein the camera external reference matrix is a matrix of a plurality of parameters, wherein the plurality of parameters are associated with the pose of the end of the robotic arm
The pose transformation for describing the terminal coordinate system of the mechanical arm relative to the camera coordinate system is expressed as follows:
in the formula,
is a 3 x 3 rotation matrix describing the rotation of the robot arm tip coordinate system with respect to the camera coordinate system,
is a translation vector of 3 multiplied by 1, which is used for describing the coordinate translation of the mechanical arm tail end coordinate system relative to the camera coordinate system;
the homogeneous transformation matrix
The pose transformation for describing the coordinate system of the tail end of the mechanical arm relative to the coordinate system of the base of the mechanical arm is expressed as follows:
in the formula,
is a 3 x 3 rotation matrix for describing the rotation of the robot arm end coordinate system relative to the robot arm base coordinate system,
is a 3 x 1 translation vector describing the translation of the robot arm tip coordinate system relative to the robot arm base coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210659413.5A CN115042175B (en) | 2022-06-10 | 2022-06-10 | Method for adjusting tail end gesture of mechanical arm of robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210659413.5A CN115042175B (en) | 2022-06-10 | 2022-06-10 | Method for adjusting tail end gesture of mechanical arm of robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115042175A true CN115042175A (en) | 2022-09-13 |
| CN115042175B CN115042175B (en) | 2024-07-16 |
Family
ID=83161834
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210659413.5A Active CN115042175B (en) | 2022-06-10 | 2022-06-10 | Method for adjusting tail end gesture of mechanical arm of robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115042175B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115319761A (en) * | 2022-10-14 | 2022-11-11 | 深圳市功夫机器人有限公司 | Control method and control system for high-precision manipulator grabbing and robot |
| CN116512286A (en) * | 2023-04-23 | 2023-08-01 | 九众九机器人有限公司 | Six-degree-of-freedom stamping robot and stamping method thereof |
| CN117817667A (en) * | 2024-01-26 | 2024-04-05 | 合肥工业大学 | Mechanical arm tail end posture adjustment method based on SVD decomposition method |
| CN118254180A (en) * | 2024-04-23 | 2024-06-28 | 合肥工业大学 | Electrical cabinet knob switch pose detection and operation method |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103418950A (en) * | 2013-07-03 | 2013-12-04 | 江南大学 | Automatic posture adjusting method for industrial welding robot in seam tracking process |
| JP2015147280A (en) * | 2014-02-07 | 2015-08-20 | キヤノン株式会社 | robot calibration method |
| CN108453727A (en) * | 2018-01-11 | 2018-08-28 | 中国人民解放军63920部队 | Mechanical arm tail end position and attitude error bearing calibration based on oval feature and system |
| US20190047152A1 (en) * | 2017-08-09 | 2019-02-14 | Omron Corporation | Calibration method, calibration system, and program |
| CN114378822A (en) * | 2022-01-19 | 2022-04-22 | 合肥工业大学 | Method for adjusting terminal pose of robot mechanical arm based on vision |
-
2022
- 2022-06-10 CN CN202210659413.5A patent/CN115042175B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103418950A (en) * | 2013-07-03 | 2013-12-04 | 江南大学 | Automatic posture adjusting method for industrial welding robot in seam tracking process |
| JP2015147280A (en) * | 2014-02-07 | 2015-08-20 | キヤノン株式会社 | robot calibration method |
| US20190047152A1 (en) * | 2017-08-09 | 2019-02-14 | Omron Corporation | Calibration method, calibration system, and program |
| CN108453727A (en) * | 2018-01-11 | 2018-08-28 | 中国人民解放军63920部队 | Mechanical arm tail end position and attitude error bearing calibration based on oval feature and system |
| CN114378822A (en) * | 2022-01-19 | 2022-04-22 | 合肥工业大学 | Method for adjusting terminal pose of robot mechanical arm based on vision |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115319761A (en) * | 2022-10-14 | 2022-11-11 | 深圳市功夫机器人有限公司 | Control method and control system for high-precision manipulator grabbing and robot |
| CN115319761B (en) * | 2022-10-14 | 2023-01-06 | 深圳市功夫机器人有限公司 | Control method and control system for high-precision manipulator grabbing and robot |
| CN116512286A (en) * | 2023-04-23 | 2023-08-01 | 九众九机器人有限公司 | Six-degree-of-freedom stamping robot and stamping method thereof |
| CN116512286B (en) * | 2023-04-23 | 2023-11-14 | 九众九机器人有限公司 | Six-degree-of-freedom stamping robot and stamping method thereof |
| CN117817667A (en) * | 2024-01-26 | 2024-04-05 | 合肥工业大学 | Mechanical arm tail end posture adjustment method based on SVD decomposition method |
| CN118254180A (en) * | 2024-04-23 | 2024-06-28 | 合肥工业大学 | Electrical cabinet knob switch pose detection and operation method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115042175B (en) | 2024-07-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115042175B (en) | Method for adjusting tail end gesture of mechanical arm of robot | |
| CN114378822B (en) | Method for adjusting tail end pose of mechanical arm of robot based on vision | |
| CN110421562B (en) | Mechanical arm calibration system and calibration method based on four-eye stereoscopic vision | |
| WO2023193362A1 (en) | Hybrid robot and three-dimensional vision based large-scale structural part automatic welding system and method | |
| CN111127568B (en) | Camera pose calibration method based on spatial point location information | |
| CN102922521B (en) | A kind of mechanical arm system based on stereoscopic vision servo and real-time calibration method thereof | |
| CN109927036A (en) | A kind of method and system of 3D vision guidance manipulator crawl | |
| CN104786226A (en) | Posture and moving track positioning system and method of robot grabbing online workpiece | |
| CN111470327B (en) | Visual stacking equipment and visual information processing method thereof | |
| CN117817667B (en) | Mechanical arm tail end posture adjustment method based on SVD decomposition method | |
| CN113379849A (en) | Robot autonomous recognition intelligent grabbing method and system based on depth camera | |
| CN113103235B (en) | Method for vertically operating cabinet surface equipment based on RGB-D image | |
| CN112577447B (en) | Three-dimensional full-automatic scanning system and method | |
| Hsu et al. | Development of a faster classification system for metal parts using machine vision under different lighting environments | |
| CN111098306A (en) | Calibration method and device of robot, robot and storage medium | |
| US11548156B2 (en) | Device and method for calibrating coordinate system of 3D camera and robotic arm | |
| CN116872216B (en) | Robot vision servo operation method based on finite time control | |
| CN113211431A (en) | Pose estimation method based on two-dimensional code correction robot system | |
| CN115972192A (en) | 3D computer vision system with variable spatial resolution | |
| Han et al. | Grasping control method of manipulator based on binocular vision combining target detection and trajectory planning | |
| Hvilshøj et al. | Calibration techniques for industrial mobile manipulators: Theoretical configurations and best practices | |
| CN110568866A (en) | Three-dimensional curved surface vision guiding alignment system and alignment method | |
| CN212578650U (en) | Detection apparatus for position appearance error of arm | |
| CN117428777A (en) | Hand-eye calibration method of bag-removing robot | |
| Fan et al. | An automatic robot unstacking system based on binocular stereo vision |
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 |