CN117145447A - Automatic rod feeding and positioning error compensation method and device for underground coal mine drilling robot - Google Patents
Automatic rod feeding and positioning error compensation method and device for underground coal mine drilling robot Download PDFInfo
- Publication number
- CN117145447A CN117145447A CN202311090972.XA CN202311090972A CN117145447A CN 117145447 A CN117145447 A CN 117145447A CN 202311090972 A CN202311090972 A CN 202311090972A CN 117145447 A CN117145447 A CN 117145447A
- Authority
- CN
- China
- Prior art keywords
- pose
- rod feeding
- error
- rod
- feeding
- 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.)
- Pending
Links
- 238000005553 drilling Methods 0.000 title claims abstract description 80
- 239000003245 coal Substances 0.000 title claims abstract description 76
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000012937 correction Methods 0.000 claims description 64
- 239000011159 matrix material Substances 0.000 claims description 26
- 238000013519 translation Methods 0.000 claims description 25
- 238000004590 computer program Methods 0.000 claims description 12
- 238000000354 decomposition reaction Methods 0.000 claims description 10
- 238000003860 storage Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 238000012360 testing method Methods 0.000 abstract description 6
- 238000006243 chemical reaction Methods 0.000 abstract description 4
- 238000013178 mathematical model Methods 0.000 abstract description 3
- 238000011156 evaluation Methods 0.000 description 25
- 238000004422 calculation algorithm Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 12
- 238000005259 measurement Methods 0.000 description 12
- 238000012840 feeding operation Methods 0.000 description 7
- 238000004458 analytical method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 5
- 238000001514 detection method Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000003754 machining Methods 0.000 description 3
- 238000012795 verification Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 229920001967 Metal rubber Polymers 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 210000000078 claw Anatomy 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 229920001971 elastomer Polymers 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000003780 insertion Methods 0.000 description 1
- 230000037431 insertion Effects 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 238000012821 model calculation Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000010079 rubber tapping Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- E—FIXED CONSTRUCTIONS
- E21—EARTH OR ROCK DRILLING; MINING
- E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
- E21B44/00—Automatic control systems specially adapted for drilling operations, i.e. self-operating systems which function to carry out or modify a drilling operation without intervention of a human operator, e.g. computer-controlled drilling systems; Systems specially adapted for monitoring a plurality of drilling variables or conditions
-
- E—FIXED CONSTRUCTIONS
- E21—EARTH OR ROCK DRILLING; MINING
- E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
- E21B19/00—Handling rods, casings, tubes or the like outside the borehole, e.g. in the derrick; Apparatus for feeding the rods or cables
- E21B19/14—Racks, ramps, troughs or bins, for holding the lengths of rod singly or connected; Handling between storage place and borehole
- E21B19/15—Racking of rods in horizontal position; Handling between horizontal and vertical position
- E21B19/155—Handling between horizontal and vertical position
Landscapes
- Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Mining & Mineral Resources (AREA)
- Physics & Mathematics (AREA)
- Environmental & Geological Engineering (AREA)
- Fluid Mechanics (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geochemistry & Mineralogy (AREA)
- Mechanical Engineering (AREA)
- Excavating Of Shafts Or Tunnels (AREA)
Abstract
The application relates to a method and a device for compensating positioning errors of an automatic rod feeding of a coal mine underground drilling robot. Then, a theoretical rod-feeding pose mathematical model is established based on space coordinate system conversion, a target rod-feeding pose is found based on a manual teaching method, the difference relation between the theoretical rod-feeding pose and the target rod-feeding pose is analyzed, and an automatic rod-feeding positioning error compensation method considering both distance and direction anisotropy is provided. Finally, through an automatic rod feeding test of the underground coal mine drilling robot, the feasibility of the method is verified, and the result shows that the method improves the accuracy of the rod feeding pose of the underground coal mine drilling robot.
Description
Technical Field
The application relates to the field of drilling robots, in particular to a method and a device for compensating positioning errors of automatic rod feeding of a drilling robot under a coal mine.
Background
Important problems such as coal seam gas extraction and water hazard control are faced in the coal mining process. Traditional underground coal mine drilling operation mainly depends on manual operation, and is high in safety risk and low in production efficiency. Therefore, researchers begin to explore a coal mine underground drilling robot, which is an intelligent robot system capable of autonomously completing drilling tasks. The technology of coal mine drilling robot application mainly comprises: automatic posture adjustment, automatic rod feeding, autonomous navigation, autonomous sensing and control and the like. The automatic rod feeding technology is one of the most critical core technologies of underground coal mine drilling robots. The automatic underground rod feeding of the coal mine means that the robot can automatically feed the drill rod to the target position, automatic rod feeding operation is achieved, and manual intervention is not needed. However, in an actual rod feeding operation, a rod feeding target position and a target rod feeding position error are often too large, resulting in a rod feeding operation failure.
Disclosure of Invention
In order to overcome at least one defect in the prior art, the application provides a method and a device for compensating the automatic rod feeding positioning error of a coal mine underground drilling robot.
In a first aspect, a method for compensating positioning error of automatic rod feeding of a drilling robot in a coal mine is provided, which comprises the following steps:
equally dividing the working space of the coal mine drilling robot into a plurality of minimum fan-shaped working ranges, wherein each minimum fan-shaped working range forms four boundary lines;
taking a plurality of boundary points on the boundary line at equal intervals for each boundary line, performing error compensation for each boundary point, and determining the position and posture error of the rod feeding of each boundary point;
aiming at any positioning point which is not on the boundary line in each minimum fan-shaped working range, firstly, drawing a cylindrical sheet body according to a rotating radius, intersecting four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, and determining the rod feeding pose error of the any positioning point by adopting an opposite distance and opposite direction based on the rod feeding pose error of the four boundary points;
and correcting the theoretical rod feeding pose based on the rod feeding pose error of the optional positioning point to obtain the corrected rod feeding pose of the optional positioning point.
In one embodiment, performing error compensation for each boundary point, determining a feed bar pose error for each boundary point includes:
theoretical rod feeding pose A, A= [ A ] of multiple boundary points on boundary line 1 ,A 2 ....A j ...A n ]Wherein A is j The theoretical rod feeding pose is the j-th boundary point, and n is the number of boundary points;
the theoretical rod feeding pose A of a plurality of boundary points is manually taught to obtain the target rod feeding pose B of the plurality of boundary points, wherein B= [ B ] 1 ,B 2 ....B j ...B n ]，B i The target rod sending pose of the jth boundary point;
calculating central point coordinates cA and cB of the theoretical rod feeding pose A and the target rod feeding pose B:
wherein cA i The coordinates of the central point coordinates cA in the i direction or angle are i=x, y, z, a, B, C; a, B, C are 3 Euler angles; a is that ij Theoretical rod feeding pose A for jth boundary point j Coordinates in the i direction or angle; cB (cB) i The coordinate of the center point coordinate cB in the i direction or angle; b (B) ij Target rod-feeding pose B for jth boundary point j Coordinates in the i direction or angle;
translating the center points of the theoretical rod feeding pose A and the target rod feeding pose B to an original point to obtain a translated theoretical rod feeding pose Ap and a translated target rod feeding pose Bp:
Ap＝A-repmat(cA,size(A,1),1)
Bp＝B-repmat(cB,size(B,1),1)
wherein, repmat is a copy and tile matrix function, and size is a function for solving the size;
Calculating a transpose matrix U:
U＝Ap'*Bp
singular value decomposition is carried out on the transposed matrix U to obtain decomposition amounts Ux and Uy; (. Cndot.)' represents transpose;
calculating a rotation matrix R and a translation matrix t:
R＝Uy*Ux'
t＝cB'-R*cA'
the theoretical rod feeding pose A is transformed to obtain a transformation result A_transformed:
A_transformed＝(R*A'+repmat(t,1,size(A,1)))'
calculating a rod feeding pose error delta A:
ΔA＝A transformed -A。
in one embodiment, for any positioning point not on the boundary line in each minimum fan-shaped working range, firstly, drawing a cylindrical sheet body according to a rotation radius, intersecting four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, determining the rod feeding pose error of any positioning point by adopting an error compensation method based on each opposite distance and opposite direction based on the rod feeding pose error of the four boundary points, and comprising the following steps:
aiming at the minimum fan-shaped working range, a set translation amount is taken as a rotation radius to draw a cylindrical sheet body, the cylindrical sheet body is intersected with the minimum fan-shaped working range to form a fan-shaped sheet body, and the fan-shaped sheet body is intersected with four boundary lines of the minimum fan-shaped working range to form four vertexes P k ，k＝1,2,3,4；
Calculating the position and posture error of a rod at any positioning point P in the fan-shaped sheet body, and adopting the following formula:
wherein Deltax ' is the feed rod pose error in the x direction, deltay ' is the feed rod pose error in the y direction, deltaz ' is the feed rod pose error in the z direction, deltax k Is the vertex P k Corresponding x-direction rod feeding pose error delta y k Is the vertex P k Corresponding y-direction rod feeding pose error, deltaz k Is the vertex P k Corresponding z-direction rod feeding pose error sigma k Is the vertex P k Corresponding feeding rod pose error weight;
wherein eta 1 ，η 2 As the weight coefficient, d k Is the vertex P k Distance from the positioning point P, lambda k For inverse distance values, ω is a weighted power exponent, β k Is triangle PP 0 P k Side PP of (a) 0 And edge PP k Included angle between P 0 Is the center of the sector piece, gamma k Is the reverse direction value.
In one embodiment, the corrected stick feed pose for each anchor point is expressed using the following formula:
wherein, (x, y, z) is the theoretical rod feeding pose of the positioning point P, deltax 'is the rod feeding pose error in the x direction, deltay' is the rod feeding pose error in the y direction, deltaz 'is the rod feeding pose error in the z direction, and (x', y ', z') is the corrected rod feeding pose of the positioning point P.
In one embodiment, the method further comprises:
correcting the sensor error, and determining a corrected length coefficient and a corrected angle coefficient:
wherein, K' l For the corrected length coefficient, L 1 、L 2 For the actual length value measured twice, n 1 、n 2 Is of actual length value L 1 、L 2 Corresponding turns of the pull rope sensor; k'. θ For the corrected angle coefficient, θ 1 、θ 2 For the actual angle value measured twice, n 3 And n 4 For an actual angle value of theta 1 、θ 2 Corresponding number of turns of the pull rope sensor.
In a second aspect, a device for compensating positioning error of automatic rod feeding of a coal mine underground drilling robot is provided, comprising:
the working range dividing module is used for equally dividing the working space of the coal mine drilling robot into a plurality of minimum fan-shaped working ranges, and each minimum fan-shaped working range forms four boundary lines;
the boundary point rod feeding pose error determining module is used for taking a plurality of boundary points on the boundary line at equal intervals for each boundary line, carrying out error compensation for each boundary point and determining the rod feeding pose error of each boundary point;
the positioning point rod-feeding pose error determining module is used for firstly drawing a cylindrical sheet body according to the rotation radius aiming at any positioning point which is not on the boundary line in each minimum fan-shaped working range, intersecting four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, and determining the rod-feeding pose error of any positioning point by adopting an opposite distance and opposite direction-based different error compensating method based on the rod-feeding pose error of the four boundary points;
and the rod feeding pose correction module is used for correcting the theoretical rod feeding pose based on the rod feeding pose error of the optional positioning point to obtain the corrected rod feeding pose of the optional positioning point.
In one embodiment, the boundary point stick posture error determination module is further configured to:
theoretical rod feeding pose A, A= [ A ] of multiple boundary points on boundary line 1 ,A 2 ....A j ...A n ]Wherein A is j The theoretical rod feeding pose is the j-th boundary point, and n is the number of boundary points;
the theoretical rod feeding pose A of a plurality of boundary points is manually taught to obtain the target rod feeding pose B of the plurality of boundary points, wherein B= [ B ] 1 ,B 2 ....B j ...B n ]，B i The target rod sending pose of the jth boundary point;
calculating central point coordinates cA and cB of the theoretical rod feeding pose A and the target rod feeding pose B:
wherein cA i The coordinates of the central point coordinates cA in the i direction or angle are i=x, y, z, a, B, C; a, B, C are 3 Euler angles; a is that ij Theoretical rod feeding pose A for jth boundary point j Coordinates in the i direction or angle; cB (cB) i The coordinate of the center point coordinate cB in the i direction or angle; b (B) ij Target rod-feeding pose B for jth boundary point j Coordinates in the i direction or angle;
translating the center points of the theoretical rod feeding pose A and the target rod feeding pose B to an original point to obtain a translated theoretical rod feeding pose Ap and a translated target rod feeding pose Bp:
Ap＝A-repmat(cA,size(A,1),1)
Bp＝B-repmat(cB,size(B,1),1)
wherein, repmat is a copy and tile matrix function, and size is a function for solving the size;
calculating a transpose matrix U:
U＝Ap'*Bp
singular value decomposition is carried out on the transposed matrix U to obtain decomposition amounts Ux and Uy; (. Cndot.)' represents transpose;
Calculating a rotation matrix R and a translation matrix t:
R＝Uy*Ux'
t＝cB'-R*cA'
the theoretical rod feeding pose A is transformed to obtain a transformation result A_transformed:
A_transformed＝(R*A'+repmat(t,1,size(A,1)))'
calculating a rod feeding pose error delta A:
ΔA＝A transformed -A。
in one embodiment, the locating point rod-feeding pose error determining module is further configured to:
aiming at the minimum fan-shaped working range, a set translation amount is taken as a rotation radius to draw a cylindrical sheet body, the cylindrical sheet body is intersected with the minimum fan-shaped working range to form a fan-shaped sheet body, and the fan-shaped sheet body is intersected with four boundary lines of the minimum fan-shaped working range to form four vertexes P k ，k＝1,2,3,4；
Calculating the position and posture error of a rod at any positioning point P in the fan-shaped sheet body, and adopting the following formula:
wherein Deltax ' is the feed rod pose error in the x direction, deltay ' is the feed rod pose error in the y direction, deltaz ' is the feed rod pose error in the z direction, deltax k Is the vertex P k Corresponding x-direction rod feeding pose error delta y k Is the vertex P k Corresponding y-direction rod feeding pose error, deltaz k Is the vertex P k Corresponding z-direction rod feeding pose error sigma k Is the vertex P k Corresponding feeding rod pose error weight;
wherein eta 1 ，η 2 As the weight coefficient, d k Is the vertex P k Distance from the positioning point P, lambda k For an inverse distance value, ω is a weighted power exponent, β k Is triangle PP 0 P k Side PP of (a) 0 And edge PP k Included angle between P 0 Is the center of the sector piece, gamma k Is the reverse direction value.
In a third aspect, a computer readable storage medium is provided, where a computer program is stored, and when the computer program is executed by a processor, the method for compensating the automatic rod feeding positioning error of the underground coal mine drilling robot is implemented.
In a fourth aspect, a computer program product is provided, including a computer program/instruction, which when executed by a processor, implements the method for compensating for automatic rod feeding positioning errors of a drilling robot in a coal mine.
Compared with the prior art, the application has the following beneficial effects: in order to improve the positioning accuracy, the pull rope sensor is calibrated at first, and the measuring accuracy is corrected. Then, a theoretical rod-feeding pose mathematical model is established based on space coordinate system conversion, a target rod-feeding pose is found based on a manual teaching method, the difference relation between the theoretical rod-feeding pose and the target rod-feeding pose is analyzed, and an automatic rod-feeding positioning error compensation method considering both distance and direction anisotropy is provided. Finally, through an automatic rod feeding test of the underground coal mine drilling robot, the feasibility of the method is verified, and the result shows that the method improves the accuracy of the rod feeding pose of the underground coal mine drilling robot.
Drawings
The application may be better understood by reference to the following description taken in conjunction with the accompanying drawings, which are incorporated in and form a part of this specification, together with the following detailed description. In the drawings:
FIG. 1 shows a schematic diagram of the kinematic structure of a coal mine downhole drilling robot;
FIG. 2 shows a block flow diagram of a method for automatic rod feeding and positioning error compensation of a coal mine downhole drilling robot according to an embodiment of the application;
FIG. 3 shows a schematic view of the working range of a coal mine downhole drilling robot;
FIG. 4 shows a schematic diagram of the relationship between the vertices of a spatial grid of a working range and anchor points;
FIG. 5 shows a schematic view of the relationship between the vertices of the segment and the anchor point P;
FIG. 6 shows a block diagram of a coal mine downhole drilling robot automatic rod feeding positioning error compensation device according to an embodiment of the application;
FIG. 7 shows a schematic diagram of the position errors of the measurement points on four boundary lines, wherein (a) is an X-direction theoretical error and a correction error, (b) is a Y-direction theoretical error and a correction error, and (c) is a Z-direction theoretical error and a correction error;
FIG. 8 shows schematic diagrams of the attitude errors of the measurement points on four boundary lines, wherein (a) is an A-angle theoretical error and a correction error, (B) is a B-angle theoretical error and a correction error, and (C) is a C-angle theoretical error and a correction error;
FIG. 9 shows a corrected position difference map;
fig. 10 shows a corrected angle difference map.
Detailed Description
Exemplary embodiments of the present application will be described hereinafter with reference to the accompanying drawings. In the interest of clarity and conciseness, not all features of an actual embodiment are described in the specification. It will of course be appreciated that in the development of any such actual embodiment, numerous implementation-specific decisions may be made to achieve the developers' specific goals, and that these decisions may vary from one implementation to another.
It should be noted here that, in order to avoid obscuring the present application due to unnecessary details, only the device structures closely related to the solution according to the present application are shown in the drawings, and other details not greatly related to the present application are omitted.
It is to be understood that the application is not limited to the described embodiments, as a result of the following description with reference to the drawings. In this context, embodiments may be combined with each other, features replaced or borrowed between different embodiments, one or more features omitted in one embodiment, where possible.
The underground coal mine drilling robot has the functions of remote control machine moving, accurate positioning, automatic drilling rod loading and unloading and the like, wherein the upright post lifting device is arranged on the crawler body platform and can drive the rotary platform to move up and down. The rotary platform can perform rotary motion around a rotary center, and a translation feeding device is arranged on the rotary platform. The translational feeding device can drive the parts such as the power head, the shackle and the like arranged on the translational feeding device to move in a translational manner along the installation surface of the rotary platform. The power head can do relative translational movement on the translational feeding device. The shackle is fixedly arranged on the translation feeding device.
When the underground coal mine drilling robot works, the control center controls the lifting device, the rotating platform and the translation feeding device to move to a proper position, so that tapping operations with different heights and different inclinations are realized, the industrial robot takes out the drill rod from the drill rod box and places the drill rod at a rod feeding position between the shackle device and the active drill rod, and automatic drill rod loading operation is realized; after the drilling is completed, the industrial robot places the drill rod into the drill rod box to complete the drill rod unloading operation.
FIG. 1 shows a schematic diagram of the movement structure of a coal mine downhole drilling robot, which is mathematically modeled according to the relationship between coordinate systems, where O w -X w Y w Z w Is the world coordinate system, O 1 -X 1 Y 1 Z 1 By industrial robotsAn industrial robot coordinate system with the bottom of the base as the center, which coincides with the world coordinate system, O 2 -X 2 Y 2 Z 2 Is a column lifting device O 3 -X 3 Y 3 Z 3 To rotate the platform coordinate system, O 4 -X 4 Y 4 Z 4 For translating the feeder coordinate system, O 5 -X 5 Y 5 Z 5 Is a power head coordinate system.
Based on space coordinate system conversion, a theoretical rod-feeding pose mathematical model is established, and the theoretical rod-feeding pose p= (x, y, z) of the target rod-feeding position under the world coordinate system is calculated by adopting the following formula:
wherein L is the motion displacement of the translation feeding device, the value is-440 mm, H is the motion displacement of the upright post lifting device, and the value is 0-400 mm. When the rotating platform rotates by an angle theta, j= -theta, wherein theta is the rotating angle value of the rotating platform, and the value is-90 degrees.
The 3 euler angles A, B, C for the corresponding industrial robot are:
when theta is greater than 0 and is equal to,
when theta is less than 0, the optical element,
when θ=0, the number of the pieces of the optical fiber,
when the underground coal mine drilling robot performs rod feeding operation, theoretical model calculation is generally performed on the underground coal mine drilling robot to obtain theoretical rod feeding pose parameters, and then the industrial robot is used for grabbing a drill rod and feeding the drill rod to a target rod feeding position. In practical application, it is found that: the phenomenon of rod feeding operation failure frequently occurs in the underground coal mine drilling robot, and research discovers that: the actual rod feeding pose is always not ideal, and is not on the same axis with the central axes of the driving drill rod of the power head and the shackle, namely, the actual rod feeding pose and the target rod feeding pose have larger deviation, so that the rod feeding operation fails, and even the actual rod feeding pose directly causes collision between the drill rod and the shackle or the driving drill rod, thereby damaging the underground drilling robot of the coal mine.
Theoretical analysis is carried out by combining the actual working condition and working environment of the underground coal mine drilling robot, and the main reasons for causing deviation between the actual rod feeding pose and the target rod feeding pose are as follows:
(1) Sensor accuracy error: the sensor used by the robot is not enough in precision, and can not provide enough accurate position information, so that the actual rod feeding pose of the underground coal mine drilling robot is inaccurate.
(2) Machining assembly errors: during processing, processing errors exist in various parts and components of the underground coal mine drilling robot to different degrees; during assembly, assembly errors exist among the parts, and the errors are more obvious on large-size parts, and cannot be eliminated, so that the errors can be reduced as much as possible.
(3) Positioning and placing errors of flexible claws: the flexible paw has certain flexibility, so that the requirement of the underground coal mine drilling robot on the position and the posture of the rod is reduced, and the accuracy of the position and the posture of the rod is reduced.
The sensor precision error can be calibrated and corrected after installation, so that the sensor detection precision is improved. After the underground coal mine drilling robot is assembled, the machining assembly error and the flexible paw positioning and placing error are basically determined to be fixed values, and the errors belong to the internal errors of the underground coal mine drilling robot, and error compensation can be carried out through a related algorithm so as to improve the accuracy of the rod feeding pose. The specific implementation mode of the automatic rod feeding and tail insertion positioning compensation method of the underground coal mine drilling robot is described in detail below.
The embodiment of the application provides a method for compensating positioning errors of automatic rod feeding of a coal mine underground drilling robot, and fig. 2 shows a flow block diagram of the method for compensating positioning errors of automatic rod feeding of the coal mine underground drilling robot according to the embodiment of the application, and referring to fig. 2, the method comprises the following steps:
And S1, equally dividing the working space of the coal mine drilling robot into a plurality of minimum fan-shaped working ranges, wherein each minimum fan-shaped working range forms four boundary lines.
Here, the actual rod feeding pose of the underground coal mine drilling robot has a deviation from the target rod feeding pose, and the deviation is easy to cause failure of rod feeding operation and even damage to underground coal mine drilling robot equipment, so that the rod feeding pose of the underground coal mine drilling robot must be corrected. However, the working range is too large, the working range is divided into a plurality of small areas, and error compensation is performed to improve the automatic rod feeding positioning precision of the underground coal mine drilling robot.
FIG. 3 shows a schematic view of the working range of the underground coal mine drilling robot, wherein the angle range is-90, the working range of the lifting upright post is 0-400 mm in the height direction, and the working range of the translation feeding device is-440 mm in the cylindrical radius.
The range of the underground coal mine drilling robot which can work is very limited under the influence of the moving range of the industrial robot arm, the height of the upright post, the power head, the roadway space, the rubber pipe and other space positions, and the solid part in the figure represents the workable space range. Because the underground coal mine drilling robot has a large span of manual working range, the whole manual teaching mode is adopted, so that the workload is huge, and the capability of improving the position and posture precision of the rod feeding is limited. In order to further improve the pose accuracy of the rod feeding, in the embodiment, working spaces of the underground coal mine drilling robot are divided according to a certain step length, the angle interval is 10 degrees, the height interval is 100mm, a series of fan-shaped working areas are formed, and four boundary lines are arranged in the radial direction of each fan-shaped working area.
And S2, taking a plurality of boundary points on each boundary line at equal intervals, performing error compensation on each boundary point, and determining the position and posture errors of the rod feeding of each boundary point.
Step S3, aiming at any positioning point which is not on the boundary line in each minimum fan-shaped working range, firstly drawing a cylindrical sheet body according to a rotating radius, intersecting four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, and determining the rod feeding pose error of any positioning point by adopting an opposite distance and opposite direction based on the rod feeding pose error of the four boundary points;
and S4, correcting the theoretical rod feeding pose based on the rod feeding pose error of the optional positioning point to obtain the corrected rod feeding pose of the optional positioning point.
In the embodiment, the working range is divided into a plurality of small areas, error compensation is performed, the error compensation is directly performed on points on the boundary line by adopting a boundary error correction compensation function, and the correction is performed on points which are not on the boundary line according to a different error compensation algorithm based on the reverse distance and the reverse direction, so that the automatic rod feeding positioning precision of the underground coal mine drilling robot is effectively improved.
In one embodiment, in step S2, performing error compensation for each boundary point, determining a beam delivery pose error of each boundary point may include:
Step S21, determining theoretical rod feeding pose A, A= [ A ] of a plurality of boundary points on boundary lines 1 ,A 2 ....A j ...A n ]Wherein A is j The theoretical rod feeding pose is the j-th boundary point, and n is the number of boundary points;
step S22, manually teaching the theoretical rod feeding pose A of the plurality of boundary points to obtain the target rod feeding pose B of the plurality of boundary points, wherein B= [ B ] 1 ,B 2 ....B j ...B n ]，B i The target rod sending pose of the jth boundary point;
step S23, calculating center point coordinates cA and cB of the theoretical rod feeding pose A and the target rod feeding pose B:
wherein cA i The coordinates of the central point coordinates cA in the i direction or angle are i=x, y, z, a, B, C; a, B, C are 3 Euler angles; a is that ij Theoretical rod feeding pose A for jth boundary point j Coordinates in the i direction or angle; cB (cB) i The coordinate of the center point coordinate cB in the i direction or angle; b (B) ij Target rod-feeding pose B for jth boundary point j Coordinates in the i direction or angle;
step S24, translating the center points of the theoretical rod feeding pose A and the target rod feeding pose B to an original point to obtain a translated theoretical rod feeding pose Ap and a translated target rod feeding pose Bp:
Ap＝A-repmat(cA,size(A,1),1)
Bp＝B-repmat(cB,size(B,1),1)
wherein, repmat is a copy and tile matrix function, and size is a function for solving the size;
step S25, calculating a transpose matrix U:
U＝Ap'*Bp
s26, singular value decomposition is carried out on the transposed matrix U, and decomposition amounts Ux and Uy are obtained; (. Cndot.)' represents transpose;
Step S27, calculating a rotation matrix R and a translation matrix t:
R＝Uy*Ux'
t＝cB'-R*cA'
step S28, the theoretical rod feeding pose A is transformed to obtain a transformation result A_transformed:
A_transformed＝(R*A'+repmat(t,1,size(A,1)))'
step S29, calculating a rod feeding pose error delta A:
ΔA＝A transformed -A。
here, the feed-rod pose error Δa of the boundary point includes a feed-rod pose error of each boundary point on the boundary line, where the feed-rod pose error of each boundary point includes feed-rod pose errors of the x-direction, y-direction, and z-direction.
In one embodiment, for boundary points on the boundary line, a boundary line error compensation function may be used to directly compensate, for points not on the boundary line, correction should be performed according to adjacent boundary line error conveying gestures, and considering that the position error vector has anisotropy, that is, two adjacent positioning points, not only the position coordinates are similar, but also the position error vector is relatively similar, for this embodiment, an anisotropic-based spatial difference error compensation method is provided. Specifically, the anisotropic spatial difference error compensation method is to divide the working area of the underground coal mine drilling robot into a series of sector grids, take the translation amount as a radius to serve as a cylindrical sheet body, find a sector sheet body W of the area where the target is located, analyze the relation between the distance and the angle between each reference point and the locating point in the sector sheet body W, and further provide a position similarity evaluation function considering the distance and the direction. Specifically, in step S3, for any positioning point not on the boundary line in each minimum fan-shaped working range, firstly, drawing a cylindrical sheet body according to a rotation radius, intersecting four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, determining a rod feeding pose error of any positioning point by adopting an error compensation method based on each opposite distance and opposite direction based on the rod feeding pose error of the four boundary points, and may include:
Step S31, for the minimum fan-shaped working range, a cylindrical sheet is drawn by taking the set rotation translation amount as a radius, the cylindrical sheet intersects with the minimum fan-shaped working range to form a fan-shaped sheet, and the fan-shaped sheet intersects with four boundary lines of the minimum fan-shaped working range to form four vertexes P k K=1, 2,3,4; here, the translation amount is a set value, and each translation is a cylindrical sheet with the set translation amount as a radius, to obtain a sector sheet.
Fig. 4 shows a schematic diagram of the relationship between the vertices of the spatial grid of the working range and the positioning points, see fig. 4, and the translation l is taken as a radius to draw a cylindrical sheet body, which is intersected with the minimum fan-shaped working range to form a fan-shaped sheet body. Four vertexes P k For calculating the error of any one of the positioning points P in the segment body, wherein P 0 Is the center of the sector piece, P 0 To P 1 、P 2 、P 3 、P 4 The distance between the two layers is consistent,by d k Representing the distances between the four vertices to the anchor point P.
FIG. 5 shows a schematic view of the relationship between the vertices of the segment and the anchor point P, P, P 0 、P k Constructing a triangle, beta k Is triangle PP 0 P k Side PP of (a) 0 And edge PP k Included angles of which k=1, 2,3,4. When the P point is positioned in different areas of the segment body, the corresponding angle beta k And distance d k And will also change with time. When the positioning point P is switched from the P 'position to the P' position, the corresponding included angle beta k ' become beta k Distance d k ' become d k ". In triangle PP 0 P k In which the angle beta can be calculated k ：
Step S32, calculating the position and posture error of the rod at any positioning point P in the fan-shaped sheet body, and adopting the following formula:
wherein Deltax ' is the feed rod pose error in the x direction, deltay ' is the feed rod pose error in the y direction, deltaz ' is the feed rod pose error in the z direction, deltax k Is the vertex P k Corresponding x-direction rod feeding pose error delta y k Is the vertex P k Corresponding y-direction rod feeding pose error, deltaz k Is the vertex P k And the corresponding position and posture errors of the rod in the z direction. Here, the vertex P k When the rod-feeding pose error delta A of the boundary line is determined, the rod-feeding pose error of each boundary point on the boundary line is obtained, and then the vertex P can be determined k Corresponding x-direction, y-direction and z-direction rod feeding pose error delta x k 、Δy k And Δz k 。
σ k Is the vertex P k Corresponding feeding rod pose error weight;
wherein eta 1 ，η 2 As the weight coefficient, a weight coefficient is used to determine the specific gravity of the inverse distance evaluation function and the inverse direction evaluation function in the total weight, and when η1=0, η2=1, the inverse direction evaluation function works, and when η1=1, η2=0, the inverse distance evaluation function works. d, d k Is the vertex P k Distance from the positioning point P, lambda k Is the inverse distance value, i.e. d k ω is a weighted power exponent, typically having a value of 1, β k Is triangle PP 0 P k Side PP of (a) 0 And edge PP k Included angle between P 0 Is the center of the sector piece, gamma k Is the opposite direction value, namely the angle beta k The cosine value of (2) plus the inverse of 1.
Further, after the rod feeding pose error of any positioning point P in the fan-shaped sheet body is obtained, the corrected rod feeding pose of each positioning point is expressed by the following formula:
wherein, (x, y, z) is the theoretical rod feeding pose of the positioning point P, deltax 'is the rod feeding pose error in the x direction, deltay' is the rod feeding pose error in the y direction, deltaz 'is the rod feeding pose error in the z direction, and (x', y ', z') is the corrected rod feeding pose of the positioning point P.
In one embodiment, the coal mine downhole drilling robot should perform verification and correction on the sensor after the sensor is installed so as to ensure the measurement accuracy of the sensor. The height lifting sensor, the translation feeding sensor and the angle sensor are all stay rope sensors. The pull rope sensor deforms deformation amounts with different lengths or angles through the metal elastomer, and the deformation can be converted into electric signals of the pull rope sensor to be output. The length or angle value is measured by reading the output signal and converting the signal to a measured value using hook law.
The length formula is measured to stay cord sensor:
L＝ΔL+L 0
wherein L is the measured length, deltaL is the deformation of the pull rope, L 0 Is the zero point length, and the calculation formula of DeltaL is as follows:
AL＝K×(n-n 0 )
k is a length coefficient, n is the number of turns of the pull rope measured by the pull rope sensor, n 0 The number of turns of the stay cord is that the stay cord sensor is at zero point length.
In this embodiment, the method further comprises:
correcting the sensor error, and determining a corrected length coefficient and a corrected angle coefficient:
wherein, K' l For the corrected length coefficient, L 1 、L 2 For the actual length value measured twice, n 1 、n 2 Is of actual length value L 1 、L 2 Corresponding turns of the pull rope sensor; k'. θ For the corrected angle coefficient, θ 1 、θ 2 For the actual angle value measured twice, n 3 And n 4 For an actual angle value of theta 1 、θ 2 Corresponding number of turns of the pull rope sensor.
Based on the same inventive concept as the automatic rod feeding and positioning error compensation method of the underground coal mine drilling robot, the embodiment also provides an automatic rod feeding and positioning error compensation device of the underground coal mine drilling robot corresponding to the automatic rod feeding and positioning error compensation device, and fig. 6 shows a block diagram of the automatic rod feeding and positioning error compensation device of the underground coal mine drilling robot according to an embodiment of the application, including:
the working range dividing module 61 is configured to equally divide the working space of the coal mine drilling robot into a plurality of minimum fan-shaped working ranges, where each minimum fan-shaped working range forms four boundary lines;
The boundary point rod-feeding pose error determining module 62 is configured to take a plurality of boundary points on the boundary line at equal intervals for each boundary line, perform error compensation for each boundary point, and determine a rod-feeding pose error of each boundary point;
the positioning point rod-feeding pose error determining module 63 is configured to firstly draw a cylindrical sheet body according to a rotation radius for any positioning point not on a boundary line in each minimum fan-shaped working range, intersect four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, and determine the rod-feeding pose error of any positioning point by adopting an opposite distance and opposite direction based on the rod-feeding pose error of the four boundary points;
the rod feeding pose correction module 64 is configured to correct the theoretical rod feeding pose based on the rod feeding pose error of the optional positioning point, and obtain a corrected rod feeding pose of the optional positioning point.
The automatic rod feeding and positioning error compensation device of the underground coal mine drilling robot and the automatic rod feeding and positioning error compensation method of the underground coal mine drilling robot have the same inventive concept, so that the specific implementation of the device can be seen from the embodiment part of the automatic rod feeding and positioning error compensation method of the underground coal mine drilling robot, and the technical effects of the device correspond to those of the method, and the detailed description is omitted herein.
In order to further verify the feasibility of the automatic rod feeding and positioning error compensation method and device for the underground coal mine drilling robot, the underground coal mine drilling robot of ZDY4500LK produced by a certain company is used as a test object, the industrial robot is EFORT-ER50A produced by EFFORT, and the drill rod is 73mm in diameter and 800mm in total length.
The industrial robot places the drill rods to be connected to a designated position through a flexible manipulator. Calibrating the proper rod feeding pose of the drill rods to be connected in a manual teaching mode, wherein the drill rods to be connected are required to be placed between the working drill rods and the driving drill rods, and the distance between the drill rods to be connected and the end face of the working female joint and the end face of the male joint of the driving drill rods are not interfered with each other; for the convenience of measurement, the front end face of the drill rod to be connected is flush with the front end face of the rear clamp holder to be used as an axial standard of the position and the posture of the drill rod; the flexible mechanical gripper is mutually perpendicular to the drill rod to be connected (industrial robot program control); in the process of placing the drill rods to be connected, the drill rods cannot collide with the two sides and the bottom of the limit groove of the rear clamp holder. When the clamping tiles are clamped, the swing amount of the drill rod to be connected is smaller than 1 degree, the absolute error of the distance between the front end face of the drill rod to be connected and the front end face of the rear clamp is smaller than 1mm, and the manual calibration of the position and the posture of the rod to be sent is considered to be successful, so that the target position and the posture of the rod to be sent at the point are obtained.
(1) Stay cord sensor verification and correction
The stay cord sensor is through measuring the length of stay cord to confirm colliery drilling robot's in pit and lift, translation, inclination's position appearance state, its detection accuracy directly influences colliery drilling robot's in pit theory and send pole position accuracy and work effect. The accuracy of the stay rope sensor is verified and corrected, so that the sensor error can be reduced, and the rod feeding accuracy of the underground coal mine drilling robot is improved.
Here, a 5m long tape is used for length measurement; the digital display inclinometer (measuring range 0-360 DEG) performs inclination angle measurement with the accuracy of 0.2 deg. And correcting and verifying the measurement result of the stay cord sensor by taking the movable limit position of the underground coal mine drilling robot as a reference point. At room temperature of 20 ℃, each group of measured values are measured 5 times on average, abnormal items are removed, and the average value is taken as a final measured result. Taking a lifting sensor as an example, when the lifting device is lowered to the lowest limit position, the number of turns of the pull rope sensor is recorded as n1, and the measured value is L1. The lifting device is lifted to the highest limit position, the number of turns of the pull rope sensor is recorded to be n2, the measured value is L2, and a measuring tape is used for measuring the distance between the highest travel point and the lowest travel point of the lifting device to be L.
Table 1 is a pull rope sensor correction parameter table, length coefficients of the sensors are corrected through a formula, table 2 is a corrected pull rope sensor detection result, analysis shows that the measured value and the measured value of the corrected pull rope sensor are basically consistent, and the sensor measurement accuracy of the underground coal mine drilling robot can be improved through checking and correcting the sensor.
Table 1 pull rope sensor correction parameter table
Table 2 corrected pull string sensor test results
(2) Automatic rod feeding pose error compensation
According to the application, a sector area is selected in the manual working range of the drilling machine for verification calculation, the lifting height is 200mm, 100mm, the inclination angle is 10 degrees and 20 degrees, and the translation amount of the translation device taken on the boundary line is-110 mm, 0mm, 220mm and 440mm. Table 3 shows the theoretical feed bar position, target feed bar position, and corrected feed bar position for the selected point on the area boundary line. Table 4 shows the position translational rotation matrices and the angular translational rotation matrices of the four boundary lines of the working area.
Fig. 7 shows a schematic diagram of the position errors of the measurement points on the four boundary lines, wherein (a) is an X-direction theoretical error and a correction error, (b) is a Y-direction theoretical error and a correction error, and (c) is a Z-direction theoretical error and a correction error, wherein the theoretical error is a difference between the target rod feeding position and the theoretical rod feeding position (represented by a solid line), and the correction error is a difference between the target rod feeding position and the corrected rod feeding position (represented by a broken line). Fig. 8 shows schematic diagrams of the attitude errors of the measurement points on the four boundary lines, wherein (a) is an a-angle theoretical error and a correction error, (B) is a B-angle theoretical error and a correction error, and (C) is a C-angle theoretical error and a correction error. As can be seen from analysis of fig. 7 and 8, the theoretical error in the X direction is 5-7 mm, and the correction error is-2 mm after correction by the algorithm; the theoretical error in the Y direction is: 11-15 mm, and the error after correction is that; -1 mm; the error before correction in the Z direction is: -6 mm-5 mm, the corrected error is: -1-2 mm; the error before the correction of the angle A is as follows: -0.6 to-0.2 mm; the corrected error is: -0.2mm; the error before the correction of the angle B is as follows: 0.5-2 mm, and the error after correction is as follows: -0.7-0.6 mm; the error of correcting the angle C is as follows: -0.6 to-0.2 mm, the error after correction is: -0.2mm to 0.1mm; the position error and the posture error are effectively compensated after being corrected, and the corrected rod feeding position and posture are closer to the target rod feeding position, so that the algorithm can greatly improve the rod feeding position precision and the posture precision.
Table 5 shows the positional error amounts of the points P1, P2, P3, and P4 when θ=17.05 °, l= 280.51mm, and h= 183.03 mm. Firstly, calculating theoretical values of points P1-P4 through pose parameters, then correcting through corresponding L1-L4 to obtain corrected parameter values, and subtracting the theoretical values from corrected parameters to obtain the error values of all vertexes.
Table 5θ=17.05 °, l= 280.51mm, h= 183.03mm, positional error amounts of points P1, P2, P3, and P4
Table 6 shows the posture error amounts of the four apex points P1, P2, P3, and P4 of the fan-shaped blade body to which the point P belongs when θ=17.05 °, l= 280.51mm, and h= 183.03 mm.
Table 6θ=17.05 °, l= 280.51mm, h= 183.03mm angular error amounts at points P1, P2, P3, and P4
Table 7 is a weight of the evaluation function, calculates the corrected pose parameters of the reverse direction evaluation function (η1=0, η2=1), corrects the pose parameters of the reverse distance evaluation function (η1=1, η2=0), and compares the influence of the reverse distance evaluation function and the reverse direction evaluation function on the correction result. Analysis of Table 7 shows that: for the inverse distance evaluation function, even if the point is very close to the positioning point, the assigned weight is not much larger than the weights assigned to other vertexes, so that error similarity is difficult to develop, the weights assigned to other vertexes are relatively larger, and the interpolation precision of the positioning point is affected. The reverse direction evaluation function is just opposite, and only the position similarity is considered, so that the influence of other vertexes on the result is ignored. For this reason, η1=0.5, η2=0.5 is selected in the present application to improve the difference accuracy.
Table 7 evaluation of function weights
Table 8 is a comparison table of the results of the distance correction, the direction correction, the reverse distance correction and the reverse direction correction, FIG. 9 shows a comparison table of the results of the difference of the correction positions, the direction correction, the reverse distance correction and the reverse direction correction angles, FIG. 9 shows a comparison table of the differences of the correction angles, FIG. 10 shows that the theoretical position and the angle deviation of the rod before correction are larger, the position and the posture errors of the rod can be greatly reduced by adopting the distance correction, the direction correction, the reverse distance correction and the reverse direction correction methods, and compared with the distance correction and the direction correction methods, the results of the reverse distance correction method and the reverse direction correction methods are more average, and the result is better because the correction method of the space difference compensation algorithm with different directions is adopted, and two weights of the reverse distance and the reverse direction are simultaneously considered.
Table 8 comparison table of various corrected position results
| Name of the name | Theory of | Actual measurement | Theoretical difference | Distance correction | Distance correction difference | Direction correction | Direction correction difference | Direction + distance correction | Direction + distance error |
| X | 384.111 | 390.231 | 6.12 | 391.145 | -0.914 | 391.289 | -1.058 | 391.217 | -0.986 |
| Y | 1345 | 1357.31 | 12.31 | 1358.007 | -0.697 | 1356.949 | 0.361 | 1357.478 | -0.168 |
| Z | 1112.8 | 1108.5 | -4.3 | 1109.024 | -0.524 | 1109.214 | -0.714 | 1109.119 | -0.619 |
Table 9 comparison table of various corrected angle results
Table 10 shows the comparison of the effects of correction for a plurality of arbitrary points using different correction methods, and analysis of table 10 reveals that: for the same locating point, the inverse distance evaluation function, the inverse direction evaluation function, the inverse distance and the inverse direction evaluation function are respectively adopted for carrying out difference compensation, and therefore, the theoretical error is larger no matter the position or the gesture is found. The error difference after the compensation of the inverse distance evaluation function and the inverse direction evaluation function is not large, the result after the compensation of the inverse distance evaluation function is better than the inverse direction evaluation function at a certain position or a certain angle of the point, and the result after the compensation of the inverse direction evaluation function is better than the inverse distance evaluation function at a certain position or a certain angle of the point. Namely, when the point position and the vertex error similarity are good, the opposite direction evaluation function plays the advantage of error similarity, the weight distribution obtained by adopting the opposite distance evaluation function is more even for the point with common error similarity, and the opposite direction evaluation function only amplifies errors, so that the smaller and better fluctuation amount of the rod-conveying pose error is required to ensure better rod-conveying effect, and the different-direction space difference compensation algorithm is adopted to reduce the fluctuation of the errors. The test shows that: the error correction algorithm based on the reverse distance and the reverse direction has universality, can be suitable for the rod feeding pose compensation of any point in the working space, and greatly improves the rod feeding accuracy of the coal mine drilling robot.
Table 10 effects of different correction methods on correction of multiple arbitrary points
/>
In summary, the application provides a positioning error compensation algorithm based on model and manual teaching, which firstly establishes a theoretical rod feeding pose model of a robot under a coal mine through space coordinate system conversion and space position relation. On the basis, the possible factors causing large rod feeding errors are analyzed, the sensor errors can be indicated to reduce the measurement errors through correction, and the machining errors of the manipulator and the flexible manipulator can be compensated and corrected through manual teaching. Dividing a working area into a plurality of fan-shaped working areas according to step length, providing a boundary line error correction compensation algorithm based on a translation rotation matrix, providing a space difference compensation algorithm based on anisotropy based on a space geometrical relationship, and finally verifying the effectiveness of the method through test comparison.
Experiments show that the anisotropic space difference compensation algorithm based on the theoretical model and the manual teaching improves the accuracy of the rod feeding position, meets the actual field use requirement, simplifies the operation flow, solidifies the operation steps, ensures that the automatic rod feeding positioning correction compensation algorithm has replicability, and realizes the mass production of the underground drilling robots of the coal mine.
The embodiment of the application provides a computer readable storage medium, wherein the computer readable storage medium stores a computer program, and when the computer program is executed by a processor, the automatic rod feeding and positioning error compensation method of the underground coal mine drilling robot is realized.
The embodiment of the application provides a computer program product, which comprises a computer program/instruction, wherein the computer program/instruction is executed by a processor to realize the automatic rod feeding and positioning error compensation method of the underground coal mine drilling robot.
The above description is merely illustrative of various embodiments of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily think about variations or substitutions within the scope of the present application, and the application is intended to be covered by the scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.
Claims (10)
1. The automatic rod feeding and positioning error compensation method for the underground coal mine drilling robot is characterized by comprising the following steps of:
equally dividing the working space of the coal mine drilling robot into a plurality of minimum fan-shaped working ranges, wherein each minimum fan-shaped working range forms four boundary lines;
Taking a plurality of boundary points on each boundary line at equal intervals, performing error compensation on each boundary point, and determining the position and posture errors of the rod feeding of each boundary point;
aiming at any positioning point which is not on a boundary line in each minimum fan-shaped working range, firstly drawing a cylindrical sheet body according to a rotating radius, intersecting four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, and determining the rod feeding pose error of the any positioning point by adopting an opposite distance and opposite direction based on the rod feeding pose error of the four boundary points;
and correcting the theoretical rod feeding pose based on the rod feeding pose error of the optional positioning point to obtain the corrected rod feeding pose of the optional positioning point.
2. The method of claim 1, wherein error compensation is performed for each boundary point, and determining a feed beam pose error for each boundary point comprises:
theoretical rod feeding pose A, A= [ A ] of multiple boundary points on boundary line 1 ,A 2 …A j …A n ]Wherein A is j The theoretical rod feeding pose is the j-th boundary point, and n is the number of boundary points;
the theoretical rod feeding pose A of the plurality of boundary points is manually taught to obtain the target rod feeding pose B of the plurality of boundary points, wherein B= [ B ] 1 ,B 2 …B j …B n ]，B i The target rod sending pose of the jth boundary point;
calculating central point coordinates cA and cB of the theoretical rod feeding pose A and the target rod feeding pose B:
wherein cA i The coordinates of the central point coordinates cA in the i direction or angle are i=x, y, z, a, B, C; a, B, C are 3 Euler angles; a is that ij Theoretical rod feeding pose A for jth boundary point j Coordinates in the i direction or angle; cB (cB) i The coordinate of the center point coordinate cB in the i direction or angle; b (B) ij Target rod-feeding pose B for jth boundary point j Coordinates in the i direction or angle;
translating the center points of the theoretical rod feeding pose A and the target rod feeding pose B to an original point to obtain a translated theoretical rod feeding pose Ap and a translated target rod feeding pose Bp:
Ap＝A-repmat(cA,size(A,1),1)
Bp＝B-repmat(cB,size(B,1),1)
wherein, repmat is a copy and tile matrix function, and size is a function for solving the size;
calculating a transpose matrix U:
U＝Ap'*Bp
singular value decomposition is carried out on the transposed matrix U to obtain decomposition amounts Ux and Uy; (. Cndot.)' represents transpose;
calculating a rotation matrix R and a translation matrix t:
R＝Uy*Ux'
t＝cB'-R*cA'
the theoretical rod feeding pose A is transformed to obtain a transformation result A_transformed:
A_transformed＝(R*A'+repmat(t,1,size(A,1)))'
calculating a rod feeding pose error delta A:
ΔA＝A transformed -A。
3. the method of claim 1, wherein for any positioning point not on the boundary line in each minimum fan-shaped working range, firstly drawing a cylindrical sheet body according to a rotation radius, wherein the cylindrical sheet body intersects with the minimum fan-shaped working range at four boundary points, determining the rod feeding pose error of the any positioning point by adopting an opposite distance and opposite direction based various error compensation method based on the rod feeding pose errors of the four boundary points, and comprising the following steps:
Aiming at the minimum fan-shaped working range, a cylinder body is drawn by taking the set translation amount as a rotation radius, the cylinder body is intersected with the minimum fan-shaped working range to form a fan-shaped body, and the fan-shaped body is intersected with four boundary lines of the minimum fan-shaped working range to form four vertexes P k ，k＝1,2,3,4；
Calculating the position and posture error of a rod at any positioning point P in the fan-shaped sheet body, and adopting the following formula:
wherein Deltax ' is the feed rod pose error in the x direction, deltay ' is the feed rod pose error in the y direction, deltaz ' is the feed rod pose error in the z direction, deltax k Is the vertex P k Corresponding x-direction feedingPosition error of the rod, deltay k Is the vertex P k Corresponding y-direction rod feeding pose error, deltaz k Is the vertex P k Corresponding z-direction rod feeding pose error sigma k Is the vertex P k Corresponding feeding rod pose error weight;
wherein eta 1 ，η 2 As the weight coefficient, d k Is the vertex P k Distance from the positioning point P, lambda k For inverse distance values, ω is a weighted power exponent, β k Is triangle PP 0 P k Side PP of (a) 0 And edge PP k Included angle between P 0 Is the center of the sector piece, gamma k Is the reverse direction value.
4. The method of claim 1, wherein the corrected stick feed pose for each anchor point is expressed using the following formula:
Wherein, (x, y, z) is the theoretical rod feeding pose of the positioning point P, deltax 'is the rod feeding pose error in the x direction, deltay' is the rod feeding pose error in the y direction, deltaz 'is the rod feeding pose error in the z direction, and (x', y ', z') is the corrected rod feeding pose of the positioning point P.
5. The method of claim 1, wherein the method further comprises:
correcting the sensor error, and determining a corrected length coefficient and a corrected angle coefficient:
wherein, K' l For the corrected length coefficient, L 1 、L 2 For the actual length value measured twice, n 1 、n 2 Is of actual length value L 1 、L 2 Corresponding turns of the pull rope sensor; k'. θ For the corrected angle coefficient, θ 1 、θ 2 For the actual angle value measured twice, n 3 And n 4 For an actual angle value of theta 1 、θ 2 Corresponding number of turns of the pull rope sensor.
6. The utility model provides a colliery is automatic pole positioning error compensation arrangement that send of drilling robot in pit which characterized in that includes:
the working range dividing module is used for equally dividing the working space of the coal mine drilling robot into a plurality of minimum fan-shaped working ranges, and each minimum fan-shaped working range forms four boundary lines;
the boundary point rod feeding pose error determining module is used for taking a plurality of boundary points at equal intervals on the boundary line for each boundary line, carrying out error compensation for each boundary point and determining the rod feeding pose error of each boundary point;
The positioning point rod feeding pose error determining module is used for firstly drawing a cylindrical sheet body according to a rotating radius aiming at any positioning point which is not on a boundary line in each minimum fan-shaped working range, intersecting four boundary points between the cylindrical sheet body and the minimum fan-shaped working range, and determining the rod feeding pose error of any positioning point by adopting an opposite distance and opposite direction based on the rod feeding pose error of the four boundary points;
and the rod feeding pose correction module is used for correcting the theoretical rod feeding pose based on the rod feeding pose error of the optional positioning point to obtain the corrected rod feeding pose of the optional positioning point.
7. The apparatus of claim 6, wherein the boundary point feed wand pose error determination module is further to:
theoretical rod feeding pose A, A= [ A ] of multiple boundary points on boundary line 1 ,A 2 …A j …A n ]Wherein A is j The theoretical rod feeding pose is the j-th boundary point, and n is the number of boundary points;
the theoretical rod feeding pose A of the plurality of boundary points is manually taught to obtain the target rod feeding pose B of the plurality of boundary points, wherein B= [ B ] 1 ,B 2 …B j …B n ]，B i The target rod sending pose of the jth boundary point;
calculating central point coordinates cA and cB of the theoretical rod feeding pose A and the target rod feeding pose B:
Wherein cA i The coordinates of the central point coordinates cA in the i direction or angle are i=x, y, z, a, B, C; a, B, C are 3 Euler angles; a is that ij Theoretical rod feeding pose A for jth boundary point j Coordinates in the i direction or angle; cB (cB) i The coordinate of the center point coordinate cB in the i direction or angle; b (B) ij Target rod-feeding pose B for jth boundary point j Coordinates in the i direction or angle;
translating the center points of the theoretical rod feeding pose A and the target rod feeding pose B to an original point to obtain a translated theoretical rod feeding pose Ap and a translated target rod feeding pose Bp:
Ap＝A-repmat(cA,size(A,1),1)
Bp＝B-repmat(cB,size(B,1),1)
wherein, repmat is a copy and tile matrix function, and size is a function for solving the size;
calculating a transpose matrix U:
U＝Ap'*Bp
singular value decomposition is carried out on the transposed matrix U to obtain decomposition amounts Ux and Uy; (. Cndot.)' represents transpose;
calculating a rotation matrix R and a translation matrix t:
R＝Uy*Ux'
t＝cB'-R*cA'
the theoretical rod feeding pose A is transformed to obtain a transformation result A_transformed:
A_transformed＝(R*A'+repmat(t,1,size(A,1)))'
calculating a rod feeding pose error delta A:
ΔA＝A transformwd -A。
8. the apparatus of claim 6, wherein the setpoint feed wand pose error determination module is further configured to:
aiming at the minimum fan-shaped working range, a cylinder body is drawn by taking the set translation amount as a rotation radius, the cylinder body is intersected with the minimum fan-shaped working range to form a fan-shaped body, and the fan-shaped body is intersected with four boundary lines of the minimum fan-shaped working range to form four vertexes P k ，k＝1,2,3,4；
Calculating the position and posture error of a rod at any positioning point P in the fan-shaped sheet body, and adopting the following formula:
wherein Deltax 'is the position and posture error of the rod in the x direction, and Deltay' is the y directionIs the feeding posture error of the Z direction, and Deltaz' is the feeding posture error of the Z direction, deltax k Is the vertex P k Corresponding x-direction rod feeding pose error delta y k Is the vertex P k Corresponding y-direction rod feeding pose error, deltaz k Is the vertex P k Corresponding z-direction rod feeding pose error sigma k Is the vertex P k Corresponding feeding rod pose error weight;
wherein eta 1 ，η 2 As the weight coefficient, d k Is the vertex P k Distance from the positioning point P, lambda k For an inverse distance value, ω is a weighted power exponent, β k Is triangle PP 0 P k Side PP of (a) 0 And edge PP k Included angle between P 0 Is the center of the sector piece, gamma k Is the reverse direction value.
9. A computer readable storage medium, wherein the computer readable storage medium stores a computer program which, when executed by a processor, implements the method for compensating for automatic rod feeding positioning errors of a coal mine downhole drilling robot according to any one of claims 1 to 5.
10. A computer program product comprising computer programs/instructions which when executed by a processor implement the method of automatic rod feed positioning error compensation for a coal mine downhole drilling robot according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311090972.XA CN117145447A (en) | 2023-08-28 | 2023-08-28 | Automatic rod feeding and positioning error compensation method and device for underground coal mine drilling robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311090972.XA CN117145447A (en) | 2023-08-28 | 2023-08-28 | Automatic rod feeding and positioning error compensation method and device for underground coal mine drilling robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117145447A true CN117145447A (en) | 2023-12-01 |
Family
ID=88900037
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311090972.XA Pending CN117145447A (en) | 2023-08-28 | 2023-08-28 | Automatic rod feeding and positioning error compensation method and device for underground coal mine drilling robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117145447A (en) |
-
2023
- 2023-08-28 CN CN202311090972.XA patent/CN117145447A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108534679B (en) | A kind of cylindrical member axis pose without target self-operated measuring unit and method | |
| CN111055273B (en) | Two-step error compensation method for robot | |
| CN105773622B (en) | A kind of industrial robot absolute precision calibration method based on IEKF | |
| CN113001535B (en) | Automatic correction system and method for robot workpiece coordinate system | |
| CN111660295A (en) | Industrial robot absolute precision calibration system and calibration method | |
| CN102198857B (en) | Assessment method for wing level of airplane based on robot and height detection unit | |
| CN108789404A (en) | A kind of serial manipulator kinematic calibration method of view-based access control model | |
| CN106777656B (en) | Industrial robot absolute accuracy calibration method based on PMPSD | |
| CN109176505B (en) | Ball arm instrument-based six-axis joint industrial robot spatial error calibration method | |
| CN104890013A (en) | Pull-cord encoder based calibration method of industrial robot | |
| CN104535027A (en) | Robot precision compensation method for variable-parameter error recognition | |
| CN111037542B (en) | Track error compensation method for linear machining of inverse dynamics control robot | |
| CN109176517B (en) | Series industrial robot connecting rod parameter calibration method based on terminal nominal point constraint | |
| CN109664328B (en) | Tool calibration method of SCARA robot | |
| CN111409067B (en) | Automatic calibration system and calibration method for robot user coordinate system | |
| WO2020125732A1 (en) | Zero calibration method and device for robot arm joint sensor | |
| CN113146620A (en) | Binocular vision-based double-arm cooperative robot system and control method | |
| CN103512511A (en) | Large face automatic measurement method based on laser tracker | |
| CN114161425B (en) | Error compensation method for industrial robot | |
| JP2023133088A (en) | Mobile flexible measurement system and measurement method for large complex component | |
| CN114474003A (en) | Vehicle-mounted construction robot error compensation method based on parameter identification | |
| CN111274696B (en) | Method for acquiring spatial position and posture of double-triangle drill boom of drill jumbo in real time | |
| CN112894814B (en) | Mechanical arm DH parameter identification method based on least square method | |
| JP5505155B2 (en) | Robot system and robot control method | |
| TW202302301A (en) | Automated calibration system and method for the relation between a profile scanner coordinate frame and a robot arm coordinate frame |
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 |