CN113858202B - Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm - Google Patents
Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm Download PDFInfo
- Publication number
- CN113858202B CN113858202B CN202111153641.7A CN202111153641A CN113858202B CN 113858202 B CN113858202 B CN 113858202B CN 202111153641 A CN202111153641 A CN 202111153641A CN 113858202 B CN113858202 B CN 113858202B
- Authority
- CN
- China
- Prior art keywords
- group
- joint
- attitude
- drill boom
- solving
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
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/1656—Programme controls characterised by programming, planning systems for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02E—REDUCTION OF GREENHOUSE GAS [GHG] EMISSIONS, RELATED TO ENERGY GENERATION, TRANSMISSION OR DISTRIBUTION
- Y02E10/00—Energy generation through renewable energy sources
- Y02E10/20—Hydro energy
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Earth Drilling (AREA)
Abstract
A method, apparatus, device and medium for inverse solution analysis of a jumbolter drill boom is provided, comprising the steps of: acquiring a target posture and a target position of an anchor rod beam of a drill boom; assigning initial values to the compensation groups of the anchor rod trolley; solving a corner of the attitude group of the anchor rod trolley according to the attitude; solving the displacement of a third position joint of the position group of the anchor rod trolley and the rotation angles of other position joints according to the tail end position and the constraint condition; judging whether the attitude group and the position group have solutions or not, if not, adjusting the compensation group, and continuously solving the attitude group and the position group until the solutions exist; and adjusting the drill boom according to the attitude group and the demodulation of the position group, reversely deducing the rotation angle or the telescopic scale of each joint of the drill boom according to the tail end position and the attitude of the drill boom, adding a design intention of the drill boom as a constraint condition in the reverse deducing process, and orderly deducing the attitude and posture of the process, so that the solution of each joint is unique, and the positioning precision of the drill boom is improved.
Description
Technical Field
The invention relates to the technical field of mechanical arm control, is applied to the field of anchor rod trolleys, and particularly relates to a reverse solution analysis method, a device and a medium for a drill arm of an anchor rod trolley.
Background
In order to realize the automatic positioning of the anchor rod trolley 8-degree-of-freedom drill boom in the operation process, the inverse kinematics of the drill boom must be analyzed, the research on the boom kinematics of the special equipment for tunnel construction at home and abroad is concentrated on the rock drilling trolley at present, and the research on the anchor rod trolley is less.
The mainstream robot inverse kinematics analysis method does not consider the design intention of the boom, and the analysis result is not in accordance with the original intention of the structural design of the boom, so that an anchor rod trolley drill arm inverse kinematics analysis method which fully considers the structural design intention is urgently needed to realize the automatic positioning of the anchor rod trolley drill arm in the operation process and provide an algorithm basis.
Disclosure of Invention
In view of the above problems in the prior art, an object of the present invention is to provide a reverse solution analysis method, device, apparatus and medium for a drill arm of a jumbo, so as to solve the problem in the prior art that an inverse kinematics analysis result is not in accordance with an original design of a boom structure of the drill arm.
In order to solve the technical problems, the specific technical scheme is as follows: in one aspect, provided herein is a method of inverse solution analysis of a drill boom of a roof bolter truck, comprising the steps of:
acquiring a target posture and a target position of an anchor rod beam of a drill boom;
assigning initial values to the compensation groups of the anchor rod trolley;
solving a corner of the attitude group of the anchor rod trolley according to the target attitude;
solving the displacement of a third position joint of the position group of the anchor rod trolley and the rotation angles of other position joints according to the target position and the constraint condition;
judging whether the attitude group and the position group have solutions or not, if not, adjusting the compensation group, and continuously solving the attitude group and the position group until the solutions exist;
and adjusting the drill boom according to the demodulation of the attitude group and the position group.
As an embodiment herein, before acquiring the target attitude and the target position of the anchor beam of the drill boom, the method includes:
acquiring a homogeneous transformation matrix of a coordinate system of adjacent joints of the drill boom according to the D-H parameters of the drill boom;
and obtaining the target posture and the target position of the anchor rod beam relative to the base of the drill boom according to the homogeneous transformation matrix of the adjacent joints.
As an embodiment herein, the obtaining the target attitude and the target position of the roof bolt beam relative to the base of the drill boom according to the homogeneous transformation matrix of the adjacent joints further comprises:
obtaining the homogeneous transformation matrix of all adjacent joints of the drill boom;
from said compensation group to said position group to attitude group
Sequentially multiplying homogeneous transformation matrixes to obtain a homogeneous transformation matrix of the anchor rod beam of the drill boom relative to the base;
extracting a rotation matrix and a position matrix of the homogeneous transformation matrix of the anchor beam of the drill boom relative to the base;
wherein the rotation matrix corresponds to the target pose and the position matrix corresponds to the target position.
As an embodiment herein, solving the rotation angle of the attitude group of the anchor trolley according to the target attitude further includes:
the attitude group comprises a first attitude joint and a second attitude joint;
the first attitude joint corresponds to a first attitude equation, and the second attitude joint corresponds to a second attitude equation;
solving the rotation angle of the second attitude joint according to the second attitude equation and the rotation matrix;
and solving the rotation angle of the first attitude joint according to the rotation angle of the second attitude, the first attitude equation and the rotation matrix.
As an embodiment herein, before solving for the displacement of the third position joint and the rotation angles of the remaining position joints of the position group of the bolting jumbo according to the target position and the constraint conditions, the method comprises:
establishing a constraint condition according to the design intention of the double-triangular oil cylinder of the drill boom;
the double-triangular oil cylinder is used for being associated with the first position joint and the fifth position joint of the position group and is also used for being connected with the second position joint and the fourth position joint of the position group.
As an embodiment herein, the establishing a constraint according to the design intent of the double-triangular cylinder of the drilling boom further includes:
correlating the rotation angles of the first position joint and the fifth position joint to establish a first correlation equation;
and correlating the rotation angles of the second position joint and the fourth position joint to establish a second correlation equation.
As an embodiment herein, the solving of the displacement of the third position joint and the rotation angle of the remaining position joints of the position group of the anchor trolley according to the target position and the constraint condition further includes:
calculating the position coordinates of a fifth position joint of the position group in the coordinate system of the base;
the third position joint corresponds to a third position equation, the second position joint corresponds to a second position equation, and the first position joint corresponds to a first position equation;
solving the displacement of the third position joint according to the third position equation, the direction coordinate and the D-H parameter;
solving a corner of a second position joint of the position group according to the second position equation, the displacement of the third position joint, the direction coordinate and the D-H parameter;
according to the first position equation, the displacement of the third position joint, the rotation angle of the second position joint, the direction coordinate and the D-H parameter, solving the rotation angle of the first position joint of the position group;
solving a rotation angle of the fifth position joint according to the first correlation equation;
and solving the rotation angle of the joint at the fourth position according to the second correlation equation.
As an embodiment herein, the calculating the position coordinates of the fifth position joint of the position group in the coordinate system of the base further includes:
multiplying a homogeneous transformation matrix of the anchor beam of the drill boom to the base by the inverse matrix of the position group to obtain a homogeneous transformation matrix of the fifth position joint relative to the base;
and obtaining the position coordinates according to the homogeneous transformation matrix of the position group relative to the base.
As an embodiment herein, the determining whether the set of poses and the set of positions have solutions further comprises:
judging whether each attitude joint of the attitude group meets the limited range of the D-H parameter of the drill boom or not;
if both the attitude group and the position group are satisfied, judging that the solutions exist in the attitude group and the position group;
and if not, judging that the attitude group and the position group have no solution.
As an embodiment herein, the adjusting the compensation set further includes:
increasing or decreasing the protrusion length of the compensation group according to the reference step size.
In another aspect, this document also provides an inverse solution analysis apparatus for a jumbolter drill boom, comprising:
the acquisition unit is used for acquiring the target attitude and the target position of the anchor rod beam of the drill boom;
the evaluation unit is used for evaluating the compensation group of the anchor rod trolley;
the attitude group solving unit is used for solving the corner of the attitude group of the anchor rod trolley according to the target attitude;
the position group solving unit is used for solving the displacement of a third position joint and the rotation angles of other position joints of the position group of the anchor rod trolley according to the target position and the constraint condition;
the judging unit is used for judging whether the attitude group and the position group have solutions or not, if not, the compensation group is adjusted, and the attitude group and the position group are continuously solved until the solutions exist;
and the adjusting unit is used for adjusting the drill boom according to the demodulation of the attitude group and the position group.
In another aspect, there is also provided a computer apparatus comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor when executing the computer program implementing a method of inverse solution analysis of a roof-bolter drill boom according to any one of the above.
In another aspect, a computer-readable storage medium is also provided herein, which stores a computer program that, when executed by a processor, implements any of the inverse solution analysis methods of a roof bolter truck drill boom.
By adopting the technical scheme, the rotation angles or the telescopic scales of all joints of the drill boom are reversely deduced according to the tail end position of the drill boom, namely the position of the anchor rod beam, in the reverse deduction process, the design intention of the drill boom is added as a constraint condition, and the deduction process poses are ordered, so that the solution of each joint is unique, and the accuracy degree of reverse deduction is improved.
In order to make the aforementioned and other objects, features and advantages of the present invention comprehensible, preferred embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the embodiments or technical solutions in the prior art, the drawings used in the embodiments or technical solutions in the prior art are briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present disclosure, and other drawings can be obtained by those skilled in the art without creative efforts.
Fig. 1 illustrates a network schematic of an inverse solution analysis apparatus of a roof bolter boom of an embodiment herein;
fig. 2 illustrates a schematic diagram of a reverse solution analysis method of a roof jumbo drill boom in an embodiment herein;
FIG. 3 illustrates a schematic diagram of a pose group solution method for an inverse solution analysis method for a jumbolter drill boom according to an embodiment herein;
FIG. 4 illustrates a position group solution method schematic of an inverse solution analysis method of a roof jumbo drill boom of an embodiment herein;
fig. 5 illustrates a schematic view of an inverse solution analysis apparatus of a roof bar trolley drill boom according to embodiments herein;
fig. 6 illustrates a schematic diagram of a matrix calculation unit of an inverse solution analysis apparatus of a jumbolter drill boom according to an embodiment herein;
fig. 7 illustrates a schematic diagram of a pose group solving unit of an inverse solution analysis apparatus of a jumbolter drill boom according to an embodiment of the present disclosure;
fig. 8 shows a schematic diagram of associated elements of an inverse solution analysis apparatus of a jumbolter drill boom according to an embodiment of the present disclosure;
fig. 9 illustrates a schematic diagram of a position group solving unit of an inverse solution analysis apparatus of a jumbolter drill boom according to an embodiment of the present disclosure;
fig. 10 illustrates a data flow diagram of an inverse solution analysis apparatus of a jumbolter drill boom of an embodiment herein;
fig. 11 shows a schematic diagram of a computer device according to an embodiment of the present disclosure.
Description of the symbols of the drawings:
101. a control terminal;
102. a drill boom;
103. a computing terminal;
501. an acquisition unit;
502. an assignment unit;
503. an attitude group solving unit;
5031. a second attitude joint solving module;
5032. a first attitude joint solving module;
504. a position group solving unit;
5041. a first position joint solving module;
5042. a second position joint solving module;
5043. a third position joint solving module;
5044. a fourth position joint solving module;
5045. a fifth position joint solving module;
505. a judgment unit;
506. an adjustment unit;
507. an association unit;
5071. a first correlation equation establishing module;
5072. a second correlation equation establishing module;
508. a matrix calculation unit;
5081. an adjacent joint matrix calculation module;
5082. a matrix multiplication calculation module;
5083. a matrix extraction module;
1102. a computer device;
1104. a processor;
1106. a memory;
1108. a drive mechanism;
1110. an input/output module;
1112. an input device;
1114. an output device;
1116. a presentation device;
1118. a graphical user interface;
1120. a network interface;
1122. a communication link;
1124. a communication bus.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments herein without making any creative effort, shall fall within the scope of protection.
It should be noted that the terms "first," "second," and the like in the description and claims herein and in the above-described drawings are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments herein described are capable of operation in sequences other than those illustrated or described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, apparatus, article, or device that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or device.
It should be noted that the method, the device, the equipment and the medium for analyzing the inverse solution of the drill arm of the anchor bar trolley can be used in the technical field of mechanical arm control, tunneling technology and any field except the technical field of mechanical arm control and tunneling technology, and the application fields of the method, the device, the equipment and the medium for analyzing the inverse solution of the drill arm of the anchor bar trolley are not limited.
Fig. 1 is a schematic network structure diagram of an inverse solution analysis apparatus for a jumbolter drill boom according to an embodiment of the present disclosure, in which an interaction method of inverse solution analysis of a jumbolter drill boom by combining a control terminal 101, a drill boom 102, and a computing terminal 103 is described, where the control terminal 101 may be connected to the drill boom 102 or the computing terminal 103 in a wired or wireless manner, respectively, the control terminal 101 may adjust a rotation angle and a telescopic length of each joint of the drill boom 102, the control terminal 101 may obtain a D-H parameter of the drill boom 102, the control terminal may analyze a limited range of the drill boom 102 according to the D-H parameter, and the computing terminal 103 may perform inverse solution operation on the drill boom and input a final calculated result to the control terminal 101.
In some embodiments of the present disclosure, the control terminal 101 may be an electronic device with a network interaction function, or may be software running in the electronic device to provide service logic for the drilling boom 102. The control terminal 101 may obtain an instruction of a user during normal operation, and send the instruction to the computing terminal 103.
In some embodiments of the present disclosure, the drill boom 102 may be a drill boom of a jumbo, a drill boom of other types of drilling vehicles, or a drill boom of a drilling vehicle and a bolting vehicle.
In some embodiments of the present description, the control terminal 101 and the computing terminal 103 may be a desktop computer, a tablet computer, a notebook computer, a smart phone, a server, or the like.
Fig. 2 is a schematic diagram of an inverse solution analysis method for a drill boom of an anchor jumbo according to an embodiment of the present invention, in which it is described that the drill boom may be a 2R-P-4R-P type drill boom, R is a rotary joint, a rotation angle of the drill boom needs to be solved, P is a telescopic joint, a telescopic length of the drill boom needs to be solved, and it is described that the method is a posture-ordered type solution, in which the drill boom is formed by sequentially combining a base to a position group, a posture group, and a compensation group, the compensation group is a telescopic joint, the position group is formed by five joints, which are a first position joint, a second position joint, a third position joint, a fourth position joint, and a fifth position joint, in which the third position joint is a telescopic joint, the first position joint, the second position joint, the fourth position joint and the fifth position joint are rotary joints, the attitude group consists of two joints which are respectively a first attitude joint and a second attitude joint, and the first attitude joint and the second attitude joint are both rotary joints, for convenience of description, the first attitude joint and the fifth position joint are adjacent, the first position joint and the fifth position joint, and the second position joint and the fourth position joint are both related through a double-triangular oil cylinder, according to the design intention embodied in the actual work of the double-triangular oil cylinder, namely the double-triangular oil cylinder can simultaneously drive the first attitude joint and the fifth attitude joint, the second attitude joint and the fourth attitude joint move, the design intention of the first attitude joint and the fifth attitude joint is used as a constraint condition to be added into inverse solution analysis, and the anchor rod beam of the drill boom, namely the position corresponding to the attitude group, the attitude of the drill boom can influence the position, after the displacement length of the compensation group is given, the attitude group of the drill boom is solved according to the tail end position of the anchor rod beam preferentially, and then the position group is solved, as required, the double-triangular oil cylinder of the drill boom drives the 4 position joints simultaneously through a hydraulic system in a manual control mode, so that the control error is large; and under the automatic control mode, the coupling control of the hydraulic system is closed, the hydraulic system of the drill boom respectively and independently controls the 4 position joints, the simultaneous control of the 4 joints is realized from a software layer by utilizing inverse solution, the manual control effect of an operator is achieved, the error is small, namely, the inverse solution algorithm of the text serves the automatic control mode of the drill boom, the respectively controlled 4 position joints achieve the intention which the double-triangular oil cylinder wants to achieve under the manual mode, and the control precision is improved, the method specifically comprises the following steps:
And step 202, assigning initial values to the compensation groups of the anchor rod trolley.
And 203, solving the corner of the attitude group of the anchor rod trolley according to the target attitude.
And 204, solving the displacement of the third position joint of the position group of the anchor rod trolley and the rotation angles of the joints at the other positions according to the target position and the constraint condition.
And step 205, judging whether the attitude group and the position group have solutions or not, if not, adjusting the compensation group, and continuously solving the attitude group and the position group until the solutions exist.
Through the steps, the drill boom with 8 degrees of freedom can be subjected to inverse solution analysis according to a pose ordered method, the inverse solution is unique, and the drill boom can be rapidly adjusted through the solution to guide the drill boom to adjust each joint.
As an embodiment herein, before step 201, further comprising:
and acquiring a homogeneous transformation matrix of the coordinate system of the adjacent joints of the drill boom according to the D-H parameters of the drill boom.
And obtaining the attitude and the tail end position of the anchor rod beam relative to the base of the drill boom according to the homogeneous transformation matrix of the adjacent joints.
In the step, the D-H parameters are improved D-H parameters proposed by Craig in 1986, the core of the D-H parameters is that 4 parameters are a torsion angle, a connecting rod length, a joint angle and a connecting rod offset, the 4 parameters uniquely determine the relative pose relationship of the joint, after the relative pose relationship of the joint is determined, the pose can be expressed in a mode of a homogeneous transformation matrix, and the pose referred to in the text is the position and the posture.
Since the range of motion of the boom is determined by its physical characteristics, the range of motion of the boom is fixed, so the D-H parameter table of the boom, such as the D-H parameter table of the anchor bar trolley boom shown in table 1, can be derived from the range of motion of each joint.
TABLE 1
In this table, α i-1 Is the connecting rod torsion angle, a i-1 Is the length of the connecting rod, theta i Is the angle of articulation, d i For the link offset, in table 1, the numbers of each component, joint and link of the drill boom are sequentially increased from the base to the tail end of the anchor beam for numbering, the base is 0, the joint i is located between the components i-1 and i, the tail end of the anchor beam is t, and the tail end of the anchor beam is also provided with a virtual joint t.
The coordinate system of the joint complies with the following convention, z i The axis being on the axis of the i-joint, x i-1 Axis in z i-1 Axis and z i On the common perpendicular to the axis.
a i-1 Is along x i-1 Axis from z i-1 Axis to z i The distance of the axes, for example the length of the common perpendicular between the first position joint and the second position joint.
α i-1 Is around x i-1 Axis from z i-1 Rotation of the shaft to z i The rotation angle of the axis is, for example, an angle required to rotate the Z axis of the coordinates of the first position joint around the X axis thereof to coincide with the Z axis of the second position joint.
d i Is along z i Axis from x i-1 Axis to x i The distance of the axes, for example, the distance between two adjacent common perpendicular lines.
θ i Is around z i Axis from x i-1 The shaft rotates to x i The rotation angle of the axis is, for example, an angle required to rotate the X axis of the coordinates of the first positional joint around the Z axis of the second positional joint to coincide with the X axis of the second positional joint.
The initial values are influenced by the calibration of the joint sensors of the drill boom, correspond to the actual structure of the drill boom, and are not related to the calculation in the text, so the detailed description is omitted.
Obtaining the homogeneous transformation matrix of the adjacent joints of the drilling boom according to the D-H parameter table of the drilling boom, and concretely deducing a formula as a more conventional technical means in the field, so the detailed description is omitted here, and obtaining the homogeneous transformation matrix of the adjacent joints of the drilling boom as
And performing matrix multiplication on the homogeneous transformation matrixes of all adjacent joints to obtain the homogeneous transformation matrix of the tail end of the drill boom relative to the drill boom base.
As an embodiment herein, the obtaining the target attitude and the target position of the anchor beam relative to the base of the drill boom according to the homogeneous transformation matrix of the adjacent joints further comprises:
obtaining the homogeneous transformation matrix of all adjacent joints of the drill boom;
performing homogeneous transformation matrix multiplication according to the sequence from the compensation group to the position group and then to the attitude group to obtain a homogeneous transformation matrix of the anchor beam of the drill boom relative to the base;
extracting a rotation matrix and a position matrix of the homogeneous transformation matrix of the anchor beam of the drill boom relative to the base;
wherein the rotation matrix corresponds to the target pose and the position matrix corresponds to the target position.
In this step, the homogeneous transformation matrices of all adjacent joints of the boom are matrix multiplied, including the virtual joints, in this context the homogeneous transformation matrices corresponding to the position groups include
A homogeneous transformation matrix corresponding to a gesture group includes>
The homogeneous transformation matrix corresponding to the compensation group is->
Virtual joint is->
The corresponding homogeneous transformation matrices may be matrix multiplied in order of position group to pose group to compensation group to virtual joint, for example as follows
Obtaining a homogeneous transformation matrix of the tail end of the drill boom relative to the base of the drill boom, wherein R represents the posture of the anchor rod beam tail end of the drill boom relative to the base, and the rotation matrix R is formed by a matrix [ n ] x n y n z ]、[o x o y o z ]And [ a ] x a y a z ]P denotes the position of the end of the roof beam of the boom relative to the base, where P is a position matrix consisting of [ P ] x p y p z ]The triangular oil cylinder is designed to complete the swing arm and the rising and falling of the large arm of the drill boom, so that the two actions do not cause the change of the posture of the anchor rod beam, and after the posture of the anchor rod beam is fixed, only the position of the anchor rod beam needs to be adjusted, so that the rotating angles of the first posture joint and the second posture joint of the posture group can be solved through R, and the displacement of the third position joint of the position group and the rotating angles of the rest position joints can be solved according to P.
As an embodiment herein, as shown in fig. 3, the schematic diagram of the attitude group solving method of the inverse solution analysis method of the jumbolter drill boom, step 203 further includes:
301, the posture group comprises a first posture joint and a second posture joint; the first attitude joint corresponds to a first attitude equation and the second attitude joint corresponds to a second attitude equation.
And step 302, solving the rotation angle of the second attitude joint according to the second attitude equation and the rotation matrix.
And step 303, solving the rotation angle of the first attitude joint according to the rotation angle of the second attitude, the first attitude equation and the rotation matrix.
In this step, the corner of the endmost second pose joint is preferentially solved according to the pose ordering method herein, according to a second pose equation q 7 ＝-arccos(n x ) In the formula, only n of the rotation matrix needs to be calculated x And the turning angle of the joint in the second posture can be obtained by the carrying-in.
After the rotation angle of the joint in the second posture is obtained, the first posture equation is used for calculating the rotation angle of the joint in the second posture
The rotation angle of the second posture joint and n of the rotation matrix y And n z And (3) substituting into a first attitude equation to obtain a corner of the first attitude joint, wherein S is a sine function, arctan2 is a double-precision floating point number, the double-precision floating point number is equivalent to a double-type parameter in the C language, the parameters before and after comma are input, and the return is an arctangent value which is represented by radian and corresponds to the position. />
As an embodiment herein, step 204 is preceded by:
and establishing constraint conditions according to the design intention of the double-triangular oil cylinder of the drill boom.
The double-triangular oil cylinder is used for connecting a first position joint and a fifth position joint of the position group and is also used for connecting a second position joint and a fourth position joint of the position group.
In the step, the first posture joint and the fifth posture joint, the second posture joint and the fourth posture joint are all related through the double-triangular oil cylinder, the first posture joint moves, the fifth posture joint moves due to the traction of the double-triangular oil cylinder, vice versa, the second posture joint moves, the fourth posture joint moves by simulating the linkage effect of the double-triangular oil cylinder, and vice versa, so that the first posture joint and the fifth posture joint serve as constraint conditions according to the characteristics of the double-triangular oil cylinder, and the operation difficulty and the order of inverse solution analysis are reduced.
As an embodiment herein, the establishing of the constraint condition according to the design intention of the double-triangle oil cylinder of the drilling boom further comprises:
and correlating the rotation angles of the first position joint and the fifth position joint to establish a first correlation equation.
And associating the rotation angles of the second position joint and the fourth position joint to establish a second association equation.
In this step, the first correlation equation is q 5 ＝-q 1 The first correlation equation reveals that the joint motion of the first position joint and the joint motion of the fifth position joint are the same in size and different in motion direction, for example, the first position joint rotates by 30 ° in the positive direction of the Z axis, and then the fifth position joint rotates by 30 ° in the negative direction of the Z axis according to the first correlation equation.
Similarly, the second correlation equation is q 4 ＝-q 2 The second correlation equation also reveals that the joint motion of the second position joint and the fourth position joint is the same in magnitude and different in motion direction, for example, the first position joint rotates by 60 degrees in the positive direction of the Z axis, and then the fourth position joint rotates by 60 degrees in the negative direction of the Z axis according to the second correlation equation.
As an embodiment herein, a schematic diagram of a position group solution method of an inverse solution analysis method of a jumbolter drill boom as shown in fig. 4, step 204 further includes:
And 402, solving the displacement of the joint at the third position according to the equation at the third position, the position coordinates and the D-H parameters.
And 403, solving a rotation angle of the second position joint of the position group according to the second position equation, the displacement of the third position joint, the position coordinate and the D-H parameter.
And 404, solving the rotation angle of the first position joint of the position group according to the displacement of the third position joint, the rotation angle of the second position joint, the position coordinate and the D-H parameter.
And 405, solving the rotation angle of the joint at the fifth position according to the first correlation equation.
And 406, solving the rotation angle of the joint at the fourth position according to the second correlation equation.
In this step, the position coordinates t of the position group in the coordinate system of the base of the drilling boom can be obtained by various mathematical transformations x 、t y And t z For example, the matrix is converted into the form of a rotation matrix, a position matrix, a perspective matrix and a proportion by multiplying the inverse matrix in two times in the equation or directly deriving the inverse matrix according to the D-H parameter and then performing matrix conversion, and the obtaining manner is not limited herein.
The third position equation herein is
Wherein a is 1 Is the distance from the joint at the first position to the joint at the second position, a 4 Is the distance from the fourth position joint to the fifth position joint, d 5 For the fifth position articulation corresponding to the length of the movement member,/ 3 All the above parameters are known for the initial expansion and contraction length of the joint at the third position, so that the displacement of the joint at the third position can be solved. The second position equation herein is
And the displacement and the known parameters of the joint at the third position are substituted into the second position equation, so that the rotating angle of the joint at the second position can be obtained.
The first position equation herein is
And substituting the displacement of the joint at the third position, the rotation angle of the joint at the second position and the known parameters into the first position equation to obtain the rotation angle of the joint at the first position.
Then the first correlation equation q is used 5 ＝-q 1 And a second correlation equation q 4 ＝-q 2 The rotation angles of the fourth position joint and the fifth position joint can be solved, and the rotation angles of all the position joints of the position group or the displacement of the third position joint can be solved through the equation.
As an embodiment herein, the calculating the position coordinates of the fifth position joint of the position group in the coordinate system of the base further includes:
and multiplying the homogeneous transformation matrix of the anchor beam of the drill boom to the base by the inverse matrix of the division position group to obtain the homogeneous transformation matrix of the fifth position joint relative to the base.
And obtaining the position coordinates according to the homogeneous transformation matrix of the position group relative to the base.
In this step, according to the formula
In the form of an inverse matrix multiplied on both sides of equal sign, e.g. < >>
Therefore/based on>
Is a unit matrix, so the right side of equal sign becomes
Using such a method ^ to the right of a equal sign>
Is removed and finally becomes
Where 0, 0 and 0 are perspective matrices O,1 is a scale, L is an identity matrix, t x 、t y And t z Is the position coordinates of the position group in the coordinate system of the base of the boom.
As an embodiment herein, step 205 further comprises:
and judging whether each attitude joint of the attitude group meets the limited range of the D-H parameter of the drill boom or not.
And if the two conditions are met, judging that the attitude group and the position group have solutions.
And if not, judging that the attitude group and the position group have no solution.
In this step, because the solution solved according to the above equation does not necessarily have a real number solution, or the range of the solution does not satisfy the actual moving range of the drill boom, it is necessary to determine whether the solution satisfies the maximum moving range specified by the D-H parameter table of the drill boom, if all joints satisfy the D-H parameter table, the inverse solution process is successful, and each joint of the drill boom can be adjusted according to the solutions, and if the joints do not satisfy the D-H parameter table, the inverse solution process fails, and the compensation group needs to be adjusted again to solve.
As an embodiment herein, step 205 further comprises:
increasing or decreasing the protrusion length of the compensation group according to the reference step size.
In this step, it should be noted that, a user may select a maximum value or a minimum value of the displacement of the compensation group to adjust when assigning an initial value to the compensation group according to an actual configuration of the drill boom, for example, the initial value of the compensation group selected by the user is the maximum value of the displacement, when the compensation group needs to be adjusted, the extension length of the compensation group needs to be continuously reduced, for example, when the initial value of the compensation group selected by the user is the minimum value of the displacement, when the compensation group needs to be adjusted, the extension length of the compensation group needs to be continuously increased, specifically, the compensation group needs to be increased or decreased, or the compensation group needs to be increased and then decreased, or the compensation group needs to be decreased and then increased first.
According to the method, the action of the triangular oil cylinder on the position joint is used as a constraint condition, the inverse solution of 8 degrees of freedom is reduced to 6 degrees of freedom for solving, a pose ordering method is adopted, the pose joint rotation angle of the pose group is solved, the displacement of the third position joint of the position group is solved, the rotation angles of the remaining four position joints are solved by using the constraint condition and an equation, and whether the solved solution meets the actual moving range of the drill boom or not is judged.
Fig. 5 is a schematic diagram of an inverse analysis device for a drill boom of a jumbolter according to an embodiment of the present disclosure, in which a basic structure of the inverse analysis device for a drill boom of a jumbolter is described, where functional units and modules may be implemented in a software manner, or may also be implemented in a general-purpose chip or a specific chip, a part or all of the functional units and modules may be on a computing terminal 103, or a part of the functional units and modules may also be on a control terminal 101, and the drill boom 102 is adjusted by cooperation with the computing terminal 103, where the system specifically includes:
an obtaining unit 501, configured to obtain a target posture and a target position of a bolt beam of a drill boom.
An assigning unit 502 for assigning an initial value to the compensation group of the anchor trolley.
And an attitude group solving unit 503, configured to solve a rotation angle of the attitude group of the anchor bar trolley according to the target attitude.
And the position group solving unit 504 is used for solving the displacement of the third position joint and the rotation angles of the other position joints of the position group of the anchor rod trolley according to the target position and the constraint condition.
And a determining unit 505, configured to determine whether the attitude group and the position group have solutions, and if not, adjust the compensation group, and continue to solve the attitude group and the position group until the solutions exist.
An adjusting unit 506, configured to adjust the drill boom according to the demodulation.
And the association unit 507 is used for establishing constraint conditions according to the design intention of the double-triangular oil cylinder of the drill boom.
And the matrix calculating unit 508 is used for calculating a homogeneous transformation matrix according to the D-H parameters of the drill boom.
As an embodiment herein, as shown in fig. 6, a schematic diagram of a matrix calculation unit of an inverse solution analysis device of a jumbolter drill boom of the embodiment herein is shown, and the matrix calculation unit 508 includes:
and the adjacent joint matrix calculation module 5081 is used for acquiring a homogeneous transformation matrix of a coordinate system of adjacent joints of the drill boom according to the D-H parameters of the drill boom.
And the matrix multiplication calculation module 5082 is used for performing homogeneous transformation matrix multiplication according to the sequence from the attitude group to the position group and then to the compensation group.
A matrix extraction module 5083 for extracting a rotation matrix and a position matrix of the homogeneous transformation matrix of the shank beam relative to the base of the drill boom.
As an embodiment herein, as shown in fig. 7, there is a schematic diagram of an attitude set solving unit of an inverse solution analyzing apparatus for a jumbolter boom of an embodiment herein, and the attitude set solving unit 503 includes:
a second pose joint solving module 5031, configured to solve the rotation angle of the second pose joint according to the second pose equation and the rotation matrix.
A first pose joint solving module 5032, configured to solve the corner of the first pose joint according to the corner of the second pose, the first pose equation, and the rotation matrix.
As an embodiment herein, as shown in fig. 8, a schematic diagram of a correlation unit of the reverse analysis device of the jumbolter drill boom of the embodiment herein is shown, and the correlation unit 507 includes:
a first correlation equation establishing module 5071, configured to correlate the rotation angles of the first position joint and the fifth position joint to establish a first correlation equation.
A second correlation equation establishing module 5072, configured to correlate the rotation angles of the second position joint and the fourth position joint, and establish a second correlation equation.
As an embodiment herein, the obtaining unit 501 is further configured to obtain position coordinates of the coordinate system of the fifth position joint in the coordinate system of the base.
As an embodiment herein, as shown in fig. 9, there is a schematic diagram of a position group solving unit of an inverse solution analyzing apparatus of a jumbolter drill boom of the embodiment herein, and the position group solving unit 504 includes:
and a third position joint solving module 5043, configured to solve the displacement of the third position joint according to the third position equation, the position coordinates, and the D-H parameter.
A second position joint solving module 5042, configured to solve a rotation angle of a second position joint of the position group according to the second position equation, the displacement of the third position joint, the direction coordinate, and the D-H parameter.
A first position joint solving module 5041, configured to solve the corner of the first position joint of the position group according to the first position equation, the displacement of the third position joint, the corner of the second position joint, the position coordinate, and the D-H parameter.
A fifth positional joint solving module 5045, configured to solve a rotation angle of the fifth positional joint according to the first correlation equation.
A fourth positional joint solving module 5044, configured to solve a rotation angle of the fourth positional joint according to the second correlation equation.
As an embodiment herein, the matrix multiplication calculation module 5082 is further configured to multiply a homogeneous transformation matrix of the anchor beam of the drill boom to the base by an inverse matrix of a posture group and a compensation group to obtain a homogeneous transformation matrix of the fifth position joint relative to the base.
A matrix extraction module 5083, further configured to obtain the position coordinate according to a homogeneous transformation matrix of the fifth position joint relative to the base.
As an embodiment herein, the adjusting unit 506 is further configured to increase or decrease the protrusion length of the compensation group according to the reference step size.
Through the device disclosed by the embodiment of the invention, the homogeneous transformation matrix of each adjacent joint of the drill boom is extracted according to the D-H parameters of the drill boom, the homogeneous transformation matrix from the tail end of the drill boom to the base is obtained according to the homogeneous transformation matrix of each joint, the rotation matrix and the position matrix are extracted from the homogeneous transformation matrix, the rotation angle of the attitude group and the displacement and rotation angle of the position group are sequentially calculated according to a plurality of equations, and the inverse solution of the drill boom with 8 degrees of freedom is realized.
Fig. 10 is a data flow chart of an inverse solution analysis apparatus for a drill boom of a jumbolter truck, which describes how a control terminal 101 performs data interaction with a drill boom 102 and a computing terminal 103, and describes how a computing terminal 103 performs an inverse solution operation, and specifically includes:
step 1001, the control terminal 101 obtains the D-H parameters of the drilling boom 102.
In this step, joint parameters of the drill boom of the anchor bar trolley can be obtained, the joint parameters are provided by the anchor bar trolley, and a table of D-H parameters, for example, a table 1 form, is established according to the range of motion of each joint indicated by the joint parameters.
Step 1002, the computing terminal 103 computes homogeneous transformation matrixes of adjacent joints according to the D-H parameters.
In the step, the calculation terminal 103 calculates the homogeneous transformation matrix of the adjacent joints after the calculation terminal 103 divides the drill boom into a position group, a posture group, a compensation group and a virtual joint from the base to the tail end of the drill boom according to the D-H parameters and classifies all the joints
And step 1003, the computing terminal 103 obtains a homogeneous transformation matrix from the tail end of the drill boom to the base of the drill boom according to the homogeneous transformation matrix of the adjacent joints.
In this step, the computing terminal 103 multiplies the homogeneous transformation matrices of the adjacent joints by the order of the position group, the attitude group, the compensation group, and the virtual joint,
wherein->
A homogeneous transformation matrix of the boom tip to the boom base.
And step 1004, the computing terminal 103 extracts a rotation matrix and a position matrix according to the homogeneous transformation matrix from the tail end of the drill boom to the base of the drill boom.
In the step, a homogeneous transformation matrix from the end of the drill boom to the base of the drill boom is simplified into the forms of a rotation matrix, a perspective matrix, a position matrix and a proportion according to a formula,
and obtaining the attitude R and the position P of the tail end of the drill boom.
In step 1005, the computing terminal 103 assigns an initial value to the compensation group.
In this step, the calculation terminal 103 selects the minimum displacement corresponding to the compensation group and assigns an initial value to the compensation group.
In step 1006, the computing terminal 103 solves the rotation angle of the attitude set according to the second attitude equation and the first attitude equation.
In this step, the computing terminal 103 calculates the second attitude equation q 7 ＝-arccos(n x ) Will rotate n of the matrix x And bringing the second attitude equation into the second attitude equation, and solving the rotation angle of the second attitude joint.
The computing terminal 103 calculates a first attitude equation
The sum of the rotation angles of the joints in the second postureN of the rotation matrix y And n z And solving the rotation angle of the first posture joint.
Step 1007, the control terminal 101 acquires the triangular oil cylinder of the drilling boom as a constraint condition.
In this step, the control terminal 101 obtains the actual joint configuration of the boom, for example, if a proportional condition or a symmetric condition is found in the triangular oil cylinder, then the condition is used as a constraint condition.
In step 1008, the computing terminal 103 extracts the constraint condition of the control terminal 101 and formulates the constraint condition.
In this step, the computing terminal will encode or mechanically characterize a formula that can be described using mathematical formulas or equations to facilitate subsequent calculations, such as formulating the relationship between the first position joint and the fifth position joint of the position group as q 5 ＝-q 1 And the relation between the second position joint and the fourth position joint is formulated as q 4 ＝-q 2 。
Step 1009, the computing terminal 103 computes a homogeneous transformation matrix of the position group relative to the drill boom base to obtain the position coordinates of the position group.
In this step, the computing terminal 103 calculates the formula
The sum of the values is expressed as ^ or ^ based on the form of the matrix of the same multiplication and inverse multiplication on both sides of equal sign>
Wherein t is x 、t y And t z Is the position coordinates of the position group in the coordinate system of the base of the boom.
Step 1010, the computing terminal 103 solves the displacement of the third position joint of the position group and the rotation angles of the joints at the other positions according to the third position equation, the second position equation, the first position equation and the constraint condition.
In this step, the computing terminal 103 brings the direction coordinates and the D-H parameters into the third position equation
The displacement of the joint at the third position is obtained.
The computing terminal 103 brings the direction coordinates, the displacement of the joint at the third position and the D-H parameters into the second position equation,
the rotation angle of the joint in the second position can be obtained.
The calculation terminal 103 brings the direction coordinate, the displacement of the joint at the third position, the rotation angle of the joint at the second position and the D-H parameter into the first position equation,
the rotation angle of the joint in the first position can be obtained.
The computing terminal 103 is based on q 5 ＝-q 1 And q is 4 ＝-q 2 The rotation angle of the fourth position joint and the rotation angle of the fifth position joint can be obtained.
In step 1011, the computing terminal 103 sends the solution to the control terminal 101.
In step 1012, the control terminal 101 determines whether the solution satisfies the limited range defined by the D-H parameter.
In this step, if yes, go to step 1013;
if not, the process returns to step 1005 after adding a certain step size to the compensation group of the computing terminal 103.
In step 1013, the control terminal 101 controls the boom 102 to adjust.
As shown in fig. 11, for a computer device provided for embodiments herein, the computer device 1102 may include one or more processors 1104, such as one or more Central Processing Units (CPUs), each of which may implement one or more hardware threads. The computer device 1102 may also include any memory 1106 for storing any kind of information, such as code, settings, data, etc. For example, and without limitation, memory 1106 may include any one or more of the following in combination: any type of RAM, any type of ROM, flash memory devices, hard disks, optical disks, etc. More generally, any memory may use any technology to store information. Further, any memory may provide volatile or non-volatile retention of information. Further, any memory may represent fixed or removable components of computer device 1102. In one case, when the processor 1104 executes the associated instructions, which are stored in any memory or combination of memories, the computer device 1102 can perform any of the operations of the associated instructions. The computer device 1102 also includes one or more drive mechanisms 1108, such as a hard disk drive mechanism, an optical disk drive mechanism, etc., for interacting with any memory.
Corresponding to the methods in fig. 2-4 and 10, the embodiments herein also provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, performs the steps of the above-described method.
Embodiments herein also provide computer readable instructions, wherein a program therein causes a processor to perform the methods as shown in fig. 2-4 and 10 when the instructions are executed by the processor.
It should be understood that, in various embodiments herein, the sequence numbers of the above-mentioned processes do not mean the execution sequence, and the execution sequence of each process should be determined by its function and inherent logic, and should not constitute any limitation to the implementation process of the embodiments herein.
It should also be understood that, in the embodiments herein, the term "and/or" is only one kind of association relation describing an associated object, meaning that three kinds of relations may exist. For example, a and/or B, may represent: a exists alone, A and B exist simultaneously, and B exists alone. In addition, the character "/" herein generally indicates that the former and latter related objects are in an "or" relationship.
Those of ordinary skill in the art will appreciate that the various illustrative components and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the components and steps of the various examples have been described above generally in terms of their functionality in order to clearly illustrate this interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the technical solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present disclosure.
It is clear to those skilled in the art that, for convenience and brevity of description, the specific working processes of the above-described systems, apparatuses and units may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the several embodiments provided herein, it should be understood that the disclosed system, apparatus, and method may be implemented in other ways. For example, the above-described apparatus embodiments are merely illustrative, and for example, the division of the units is only one logical division, and other divisions may be realized in practice, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may also be an electrical, mechanical or other form of connection.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one position, or may be distributed on multiple network units. Some or all of the units can be selected according to actual needs to achieve the purposes of the embodiments herein.
In addition, functional units in the embodiments herein may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present invention may be implemented in a form of a software product, which is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the methods described in the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
It should be noted that, the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, presented data, etc.) referred to in the present application are information and data authorized by the user or sufficiently authorized by each party.
The principles and embodiments of this document are explained herein using specific examples, which are presented only to aid in understanding the methods and their core concepts; meanwhile, for the general technical personnel in the field, according to the idea of this document, there may be changes in the concrete implementation and the application scope, in summary, this description should not be understood as the limitation of this document.
Claims (12)
1. An inverse solution analysis method of a drill boom of an anchor bar trolley is characterized by comprising the following steps of:
acquiring a target posture and a target position of an anchor rod beam of a drill boom;
assigning initial values to the compensation groups of the anchor rod trolley;
solving a corner of the attitude group of the anchor rod trolley according to the target attitude;
establishing constraint conditions according to the design intention of the double-triangular oil cylinder of the drill boom;
the double-triangular oil cylinder is used for associating a first position joint and a fifth position joint of a position group and is also used for connecting a second position joint and a fourth position joint of the position group;
solving the displacement of a third position joint of the position group of the anchor rod trolley and the rotation angles of other position joints according to the target position and the constraint condition;
judging whether the attitude group and the position group have solutions or not, if not, adjusting the compensation group, and continuously solving the attitude group and the position group until the solutions exist;
and adjusting the drill boom according to the demodulation of the attitude group and the position group.
2. The inverse solution analysis method of a jumbo drill boom of claim 1, prior to obtaining the target attitude and the target position of the anchor beam of the drill boom, comprising:
acquiring a homogeneous transformation matrix of a coordinate system of adjacent joints of the drill boom according to the D-H parameters of the drill boom;
and obtaining the target posture and the target position of the anchor rod beam relative to the base of the drill boom according to the homogeneous transformation matrix of the adjacent joints.
3. The inverse solution analysis method of the bolting jumbo drill boom of claim 2, wherein said obtaining said target attitude and said target position of said bolting beam relative to a base of said drill boom according to said homogeneous transformation matrix of adjacent joints, further comprises:
obtaining the homogeneous transformation matrix of all adjacent joints of the drill boom;
performing homogeneous transformation matrix multiplication according to the sequence from the position group to the attitude group and then to the compensation group to obtain a homogeneous transformation matrix of the anchor beam of the drill boom relative to the base;
extracting a rotation matrix and a position matrix of the homogeneous transformation matrix of the anchor beam of the drill boom relative to the base;
wherein the rotation matrix corresponds to the target pose and the position matrix corresponds to the target position.
4. The inverse solution analysis method of the bolting carriage drill boom of claim 3, wherein said solving corners of a set of poses of said bolting carriage from said target pose further comprises:
the attitude group comprises a first attitude joint and a second attitude joint;
the first attitude joint corresponds to a first attitude equation, and the second attitude joint corresponds to a second attitude equation;
solving the rotation angle of the second attitude joint according to the second attitude equation and the rotation matrix;
and solving the corner of the first attitude joint according to the corner of the second attitude joint, the first attitude equation and the rotation matrix.
5. The inverse solution analysis method of the jumbolter drill boom of claim 2, wherein the establishing of the constraint condition according to the design intent of the double triangular cylinder of the drill boom further comprises:
correlating the rotation angles of the first position joint and the fifth position joint to establish a first correlation equation;
and associating the rotation angles of the second position joint and the fourth position joint to establish a second association equation.
6. The method of inverse solution analysis of a bolting jumbo drill boom of claim 5, wherein said solving for displacements of a third position joint and corners of remaining position joints of a position group of said bolting jumbo from said target position and constraints further comprises:
calculating the position coordinates of a fifth position joint of the position group in the coordinate system of the base;
the third position joint corresponds to a third position equation, the second position joint corresponds to a second position equation, and the first position joint corresponds to a first position equation;
solving the displacement of the third position joint according to the third position equation, the position coordinate and the D-H parameter;
according to the second position equation, the displacement of the third position joint, the position coordinate and the D-H parameter, solving a corner of a second position joint of the position group;
according to the first position equation, the displacement of the third position joint, the rotation angle of the second position joint, the position coordinates and the D-H parameter, solving the rotation angle of the first position joint of the position group;
solving the rotation angle of the joint at the fifth position according to the first correlation equation;
and solving the rotation angle of the joint at the fourth position according to the second correlation equation.
7. The inverse solution analysis method for a jumbo drill boom of claim 6, wherein the calculating the position coordinates of the fifth position joint of the position group in the coordinate system of the base further comprises:
multiplying a homogeneous transformation matrix of the anchor beam of the drill boom to the base by the inverse matrix of the position group to obtain a homogeneous transformation matrix of the fifth position joint relative to the base;
and obtaining the position coordinates according to the homogeneous transformation matrix of the position group relative to the base.
8. The method of claim 2, wherein the determining whether the attitude group and the position group have solutions further comprises:
judging whether each attitude joint of the attitude group meets the limiting range of the D-H parameter of the drill boom or not;
if both are satisfied, judging that the attitude group and the position group have solutions;
and if not, judging that the attitude group and the position group have no solution.
9. The method of inverse solution analysis of a roof-bolter truck drill boom of claim 1, wherein said adjusting said compensation group further comprises:
increasing or decreasing the protrusion length of the compensation group according to the reference step size.
10. The utility model provides a reverse analytical equipment that separates of stock platform truck drill boom which characterized in that includes:
the acquisition unit is used for acquiring the target posture and the target position of the anchor rod beam of the drill boom;
the assignment unit is used for assigning an initial value to the compensation group of the anchor rod trolley;
the attitude group solving unit is used for solving the corner of the attitude group of the anchor rod trolley according to the target attitude;
the position group solving unit is used for establishing a constraint condition according to the design intention of the double-triangular oil cylinder of the drilling boom;
the double-triangular oil cylinder is used for associating a first position joint and a fifth position joint of the position group and is also used for connecting a second position joint and a fourth position joint of the position group; the system is also used for solving the displacement of a third position joint of the position group of the anchor rod trolley and the rotation angles of other position joints according to the target position and the constraint condition;
the judging unit is used for judging whether the attitude group and the position group have solutions or not, if not, the compensation group is adjusted, and the attitude group and the position group are continuously solved until the solutions exist;
and the adjusting unit is used for adjusting the drill boom according to the demodulation of the attitude group and the position group.
11. A computer arrangement comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor, when executing the computer program, implements a method of inverse solution analysis of a roof-bolter drill boom according to any of claims 1-9.
12. A computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program which, when executed by a processor, implements the method for inverse solution analysis of a roof jumbo drill boom according to any one of claims 1 to 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111153641.7A CN113858202B (en) | 2021-09-29 | 2021-09-29 | Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111153641.7A CN113858202B (en) | 2021-09-29 | 2021-09-29 | Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113858202A CN113858202A (en) | 2021-12-31 |
| CN113858202B true CN113858202B (en) | 2023-04-11 |
Family
ID=79000693
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111153641.7A Active CN113858202B (en) | 2021-09-29 | 2021-09-29 | Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113858202B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117077384A (en) * | 2023-08-01 | 2023-11-17 | 中铁隧道局集团有限公司 | Inverse solution method for anchor rod pose at tail end of mechanical arm of anchor rod trolley |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103394725B (en) * | 2013-03-25 | 2016-02-03 | 河北科技大学 | A kind of drill carriage automatic punching method |
| CN107066645A (en) * | 2016-12-01 | 2017-08-18 | 西北工业大学 | A kind of seven freedom biasing mechanism arm is against solution method |
| CN107885916A (en) * | 2017-10-27 | 2018-04-06 | 西安工业大学 | A kind of drill jumbo drill boom Analytical Methods of Kinematics based on CFDH methods |
| CN108789406A (en) * | 2018-06-05 | 2018-11-13 | 上海大学 | A kind of inverse solution method for solving for redundant mechanical arm approachability analysis |
| CN111958611B (en) * | 2020-07-31 | 2021-10-08 | 华中科技大学 | Milling robot posture optimization method and device considering minimum contour error |
| CN112536792B (en) * | 2020-11-24 | 2022-03-15 | 河南理工大学 | Robot arm inverse solution method based on spherical geometric analysis method |
-
2021
- 2021-09-29 CN CN202111153641.7A patent/CN113858202B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113858202A (en) | 2021-12-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| DE112019002310B4 (en) | PERFORMING A PEG IN HOLE TASK WITH AN UNKNOWN SLOPE | |
| CN101204813B (en) | Device and method for robot offline programming | |
| CN104215206B (en) | Base coordinate calibration method of two-robot collaboration system | |
| EP3733355A1 (en) | Robot motion optimization system and method | |
| CN107885916A (en) | A kind of drill jumbo drill boom Analytical Methods of Kinematics based on CFDH methods | |
| CN113858202B (en) | Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm | |
| CN108673505A (en) | A kind of mechanical arm tail end precise motion control method | |
| DE102020115631A1 (en) | Machine learning device, robot system and machine learning method for learning a state of a tool | |
| CN104460698A (en) | UVW platform calibrating method and device | |
| CN103394725B (en) | A kind of drill carriage automatic punching method | |
| CN115648228A (en) | Industrial robot multi-source error compensation method, device, equipment and storage medium | |
| CN117891282B (en) | Method and device for controlling reverse movement of drill boom of drill jumbo without tag data | |
| CN117306614A (en) | Excavator bucket operation track determination method, device, equipment and storage medium | |
| CN111805540A (en) | Method, device and equipment for determining workpiece grabbing pose and storage medium | |
| CN108115672B (en) | Motion control system and method of oblique wrist spraying robot | |
| DE102019200435A1 (en) | Method for providing a kinematic model for kinematic work systems | |
| CN112613196A (en) | Five-axis robot posture inverse solution method and related equipment | |
| CN110515884B (en) | Construction site reinforcing bar range unit based on image analysis | |
| CN111075489A (en) | Attitude description method for floating connection mechanism of hydraulic support and scraper conveyor | |
| Šumanas et al. | Determination of excavator tool position using absolute sensors | |
| Kyrylovych et al. | Automated Attestation of Metrics for Industrial Robots’ Manipulation Systems | |
| KR101042085B1 (en) | Robotic drilling system and the method | |
| CN116277009A (en) | Three-joint mechanical arm motion control method and device, electronic equipment and storage medium | |
| CN114233266A (en) | Automatic control method and system for drilling machine and storage medium | |
| DETEȘAN et al. | A THEORETICAL APPROACH ON DETERMINING THE GEOMETRICAL ERRORS IN CASE OF ARTICULATED ROBOT STRUCTURES |
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 |