CN113954081A - Working arm track planning method and system of anchor rod support robot - Google Patents
Working arm track planning method and system of anchor rod support robot Download PDFInfo
- Publication number
- CN113954081A CN113954081A CN202111483342.XA CN202111483342A CN113954081A CN 113954081 A CN113954081 A CN 113954081A CN 202111483342 A CN202111483342 A CN 202111483342A CN 113954081 A CN113954081 A CN 113954081A
- Authority
- CN
- China
- Prior art keywords
- joint
- discrete
- marked
- working arm
- determining
- 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
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000001133 acceleration Effects 0.000 claims description 21
- 239000013598 vector Substances 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 13
- 239000003550 marker Substances 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 8
- 239000003245 coal Substances 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000005553 drilling Methods 0.000 description 2
- 238000004873 anchoring Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000005641 tunneling Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a working arm track planning method and a working arm track planning system of a bolting robot, which relate to the technical field of automatic bolting, and the working arm track planning method comprises the following steps: determining the starting and ending positions of the marking joints according to the first anchor hole position and the second anchor hole position; determining an obstacle-free running track of the marked joint according to the starting and ending positions of the marked joint; discrete processing is carried out on the barrier-free running track of the marked joint, and the number of discrete sections of the barrier-free running track of the marked joint and the discrete time corresponding to each discrete section are determined; planning the driving path of each residual joint according to the segmented polynomial algorithm, the number of the discrete segments and the discrete time corresponding to each discrete segment, and determining the driving track of the residual joint. By adopting a piecewise polynomial interpolation algorithm, the invention ensures the smooth motion track of the working arm of the anchor bolt supporting robot.
Description
Technical Field
The invention relates to the technical field of automatic anchor rod support, in particular to a working arm track planning method and system of an anchor rod support robot.
Background
The anchor bolt support robot is important equipment for supporting a coal mine tunnel, and has great significance for efficient and safe tunneling of the tunnel. The working arm trajectory planning of the bolting robot is the technical basis of automatic bolting. The trajectory planning of the working arm of the anchor-supported robot is expressed as a function of the motion amount of displacement, speed, acceleration and other gestures with respect to time, and is generally planned and expressed in joint space or Cartesian space. Due to the complex working environment of the underground coal mine, the anchor rod support robot has high requirements on the operation of the anchor rod support robot. The working arm of the anchor bolt support robot needs to reasonably avoid obstacles in the external environment in the movement process and simultaneously needs to ensure stable movement and no impact.
At present, point-to-point trajectory planning is generally adopted for trajectory planning of a working arm of the anchor bolt support robot, but the point-to-point trajectory planning cannot meet the requirements at the same time, and the working arm is easy to collide with a machine body or an external environment when moving between two anchor holes, so that serious potential safety hazards are brought to underground safety support.
Disclosure of Invention
The invention aims to provide a method and a system for planning a track of a working arm of an anchor bolt support robot, which ensure that the movement track of the working arm of the anchor bolt support robot is smooth.
In order to achieve the purpose, the invention provides the following scheme:
a working arm track planning method of a bolting robot comprises the following steps:
determining the starting and ending positions of the marking joints according to the first anchor hole position and the second anchor hole position; the marking joint is any joint of the working arm;
determining an obstacle-free running track of the marked joint according to the starting and ending positions of the marked joint;
discrete processing is carried out on the barrier-free running track of the marked joint, and the number of discrete sections of the barrier-free running track of the marked joint and the discrete time corresponding to each discrete section are determined;
planning a driving path of each residual joint according to a segmented polynomial algorithm, the number of the discrete segments and the discrete time corresponding to each discrete segment, and determining a driving track of the residual joint; the remaining joints are any joints of the working arm except the marking joint.
Optionally, the discrete-time calculation process specifically includes:
according to the formulaCalculating the discrete time corresponding to the n discrete section of the marked joint;
wherein v ismaxSpeed constraint for maximum speed of working arm, PnFor the end path point, P, of the n-th discrete segment of the marked jointn-1Is the starting path point of the nth discrete segment of the marked joint.
Optionally, the planning a driving path of each remaining joint according to a piecewise polynomial algorithm, the number of discrete segments, and a discrete time corresponding to each discrete segment, and determining a driving trajectory of the remaining joint specifically includes:
according to the formula
Planning the driving path of each residual joint and determining the residual jointsThe travel track of the remaining joints;
wherein s isi(t) is the travel locus of the ith remaining joint, sin(t) is the driving track of the ith residual joint in the nth discrete section, tnThe discrete time corresponding to the nth discrete segment of the marked joint is n, which is 1,2, …, k, k is the number of discrete segments; a isi1m、ai2l、…、ai(k-1)l、aikmAre coefficient vectors, m is 0,1,2,3,4, l is 0,1,2, 3.
Optionally, the determining the starting and ending positions of the marked joint according to the first anchor hole position and the second anchor hole position specifically includes:
acquiring a first anchor hole position and a second anchor hole position;
calculating the first anchor hole position based on an inverse kinematics model to determine a starting position of a marked joint; the inverse kinematics model is determined by a kinematics model of the working arm; the kinematic model of the working arm is a six-degree-of-freedom working arm motion model determined by an MD-H parameter method;
calculating the second anchor eye position based on the inverse kinematics model to determine an end position of a marker joint.
Optionally, the determining an obstacle-free travel track of the marked joint according to the starting and ending position of the marked joint specifically includes:
determining velocity information for the marked joint;
determining an obstacle-free running track of the marked joint according to the starting and ending position and the speed information of the marked joint;
wherein the speed information comprises: the joint velocity of the start position of the marking joint is 0, the joint acceleration of the start position of the marking joint is 0, the joint velocity of the end position of the marking joint is 0, and the joint acceleration of the end position of the marking joint is 0.
Optionally, the calculating process of the coefficient vector specifically includes:
wherein, wherein n is an element of [2, k-1 ]],Q is an information set; the information set comprises the starting and ending positions of the marked joints, the joint speeds of the starting positions of the marked joints, the joint accelerations of the starting positions of the marked joints, the joint speeds of the ending positions of the marked joints, the joint accelerations of the ending positions of the marked joints and the head and tail positions of the marked joints in each discrete segment.
In order to achieve the purpose, the invention also provides the following technical scheme:
a working arm trajectory planning system for a bolting robot, comprising:
the starting and ending position determining module is used for determining the starting and ending position of the marking joint according to the first anchor hole position and the second anchor hole position; the marking joint is any joint of the working arm;
the barrier-free track determining module is used for determining a barrier-free running track of the marked joint according to the starting and ending positions of the marked joint;
the discrete processing module is used for performing discrete processing on the barrier-free running track of the marked joint and determining the number of discrete sections of the barrier-free running track of the marked joint and the discrete time corresponding to each discrete section;
the joint driving track determining module is used for planning the driving path of each residual joint according to a segmented polynomial algorithm, the number of discrete segments and the discrete time corresponding to each discrete segment, and determining the driving track of the residual joint; the remaining joints are joints in the working arm other than the marked joint.
Optionally, in terms of determining the discrete time, the discrete processing module further includes:
according to the formulaCalculating the discrete time corresponding to the n discrete section of the marked joint;
wherein v ismaxSpeed constraint for maximum speed of working arm, PnFor the end path point, P, of the n-th discrete segment of the marked jointn-1Is the starting path point of the nth discrete segment of the marked joint.
Optionally, the joint driving trajectory determining module specifically includes:
a driving track calculation submodule for calculating a driving track according to a formula
Planning a driving path of each residual joint and determining a driving track of the residual joint;
wherein s isi(t) is the travel locus of the ith remaining joint, sin(t) is the driving track of the ith residual joint in the nth discrete section, tnThe discrete time corresponding to the nth discrete segment of the marked joint is n, which is 1,2, …, k, k is the number of discrete segments; a isi1m、ai2l、…、ai(k-1)l、aikmAre coefficient vectors, m is 0,1,2,3,4, l is 0,1,2, 3.
Optionally, the starting and ending position determining module specifically includes:
the anchor hole position determining submodule is used for acquiring a first anchor hole position and a second anchor hole position;
a starting position calculation submodule for calculating the first anchor eye position based on an inverse kinematics model to determine a starting position of a marker joint; the inverse kinematics model is determined by a kinematics model of the working arm; the kinematic model of the working arm is a six-degree-of-freedom working arm motion model determined by an MD-H parameter method;
and the termination position calculation submodule is used for calculating the second anchor hole position based on the inverse kinematics model so as to determine the termination position of the marking joint.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
firstly, determining the starting and ending positions of the marked joints, and determining the barrier-free running track of the marked joints according to the starting and ending positions of the marked joints; then carrying out discrete processing on the barrier-free running track of the marked joint, and determining the number of discrete sections of the barrier-free running track of the marked joint and the discrete time corresponding to each discrete section; and finally planning the driving path of each residual joint according to the piecewise polynomial algorithm, the number of the discrete segments and the discrete time corresponding to each discrete segment, and determining the driving track of the residual joint. The invention realizes the piecewise polynomial interpolation of the track of the working arm of the anchor bolt support robot so as to ensure the smooth and steady motion of the track of the working arm of the anchor bolt support robot. Meanwhile, by adopting a piecewise polynomial interpolation method, the collision problem possibly existing in point-to-point trajectory planning is solved, and the safety of anchor bolt support is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
Fig. 1 is a schematic flow chart of a working arm trajectory planning method of an anchor bolt support robot according to the present invention;
fig. 2 is a schematic structural diagram of a working arm trajectory planning system of the bolting robot according to the invention;
FIG. 3 is a diagram showing a joint movement locus of the joint 1 according to the third embodiment of the present invention;
FIG. 4 is a velocity chart of the joint movement of the joint 1 according to the third embodiment of the present invention;
FIG. 5 is a diagram showing the acceleration of joint movement of the joint 1 according to the third embodiment of the present invention;
fig. 6 is a diagram illustrating the movement path of the end of the working arm according to the third embodiment of the present invention.
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 given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention aims to provide a working arm track planning method and a working arm track planning system for an anchor bolt support robot, which can overcome the defects existing in point-to-point track planning, can pass through all preset path points, reduce the possibility of collision, and simultaneously ensure that the track, the speed and the acceleration of the anchor bolt support robot are continuous, the motion is stable and no impact exists on the premise that the anchor bolt support robot meets constraint conditions.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
Example one
As shown in fig. 1, the present embodiment provides a working arm trajectory planning method for an anchor bolt support robot, where the working arm trajectory planning method includes:
step 1001, a first anchor eye position and a second anchor eye position are obtained. Specifically, the positions of a roadway top plate and a side wall where anchor rods need to be installed are identified through a vision module of the anchor rod supporting robot, and then a first anchor hole position and a second anchor hole position in a world coordinate system with the anchor rod supporting robot as an original point are determined.
Step 1002, calculating the first anchor hole position based on an inverse kinematics model to determine the initial position of the marking joint; the inverse kinematics model is determined by a kinematics model of the working arm; the kinematic model of the working arm is a six-degree-of-freedom working arm motion model determined by an MD-H parameter method. In particular, the marking joint is a joint on the drill rig at the end of the working arm.
The six-freedom-degree kinematics model of the anchor bolt support robot working arm isWherein,is a posture matrix of the drill frame at the tail end of the working arm,is the position vector of the drill bit on the end drilling rig of the working arm. Then, according to the kinematic model of the working arm, an inverse kinematic model of the working arm can be obtained, in other words, the position and the posture of a drill bit arranged on a drilling rig at the tail end of the working arm are known, and then the corresponding joint coordinates at the tail end of the working arm can be obtained.
Step 1003, calculating the second anchor hole position based on the inverse kinematics model to determine the termination position of the marked joint.
And 200, determining the barrier-free running track of the marked joint according to the starting and ending positions of the marked joint. Specifically, the step 200 specifically includes:
in step 2001, velocity information of the marked joint is determined.
Step 2002, determining an obstacle-free running track of the marked joint according to the starting and ending position and the speed information of the marked joint; wherein the speed information comprises: the joint speed of the initial position of the marking joint is 0, the joint acceleration of the initial position of the marking joint is 0, the joint speed of the end position of the marking joint is 0, and the joint acceleration of the end position of the marking joint is 0, so that the motion stability of the working arm is ensured, and no impact exists at the starting position and the stopping position.
Specifically, the first anchor hole position and the second anchor hole position are two adjacent anchoring positions on the roadway working surface. According to the obstacle avoidance requirement of the working arm of the anchor bolt supporting robot, the planning module of the anchor bolt supporting robot plans an obstacle-free path by taking the joint coordinate of the marked joint corresponding to the first anchor hole position as the initial position of the marked joint and taking the joint coordinate of the marked joint corresponding to the second anchor hole position as the final position of the marked joint.
And 300, performing discrete processing on the barrier-free running track of the marked joint, and determining the number of discrete sections of the barrier-free running track of the marked joint and the discrete time corresponding to each discrete section. Specifically, the discrete time calculation process specifically includes:
according to the formulaAnd calculating the discrete time corresponding to the mark joint in the nth discrete segment.
Wherein v ismaxSpeed constraint for maximum speed of working arm, PnFor the end path point, P, of the n-th discrete segment of the marked jointn-1Is the starting path point of the nth discrete segment of the marked joint.
In particular, the unobstructed travel path of the marked joint is discretized into a number of path points [ P ]0,P1,P2,P3,…,Pn-1,Pn]Wherein P is0To mark the starting path point of the joint, PnTo mark the end path points of the joint, [ P ]0,P1]To mark a first discrete segment of a joint, [ P ]n-1,Pn]And calculating the joint coordinates of the marked joints corresponding to the path points based on the inverse kinematics model of the working arm.
Planning a driving path of each residual joint and determining a driving track of the residual joint; wherein s isi(t) is the travel locus of the ith remaining joint, sin(t) is the driving track of the ith residual joint in the nth discrete section, tnThe discrete time corresponding to the nth discrete segment of the marked joint is n, which is 1,2, …, k, k is the number of discrete segments; a isi1m、ai2l、…、ai(k-1)l、aikmAre coefficient vectors, m is 0,1,2,3,4, l is 0,1,2, 3.
Further, the calculating process of the coefficient vector specifically includes:
wherein, wherein n is an element of [2, k-1 ]],Q is an information set; the letterThe information set comprises the starting and ending positions of the marked joints, the joint speeds of the starting positions of the marked joints, the joint accelerations of the starting positions of the marked joints, the joint speeds of the ending positions of the marked joints, the joint accelerations of the ending positions of the marked joints and the head and tail positions of the marked joints in each discrete segment. Specifically, the joint velocity of the starting position of the marked joint, the joint acceleration of the starting position of the marked joint, the joint velocity of the ending position of the marked joint, the joint acceleration of the ending position of the marked joint and the head-tail position of the marked joint in each discrete segment are represented in a vector form.
Finally substituting the coefficient vector obtained by resolving into a formula
And 4-3- … -3-4 segmented polynomial tracks of joints of the anchor rod supporting robot can be obtained, and further a first derivative and a second derivative are obtained to obtain speed and acceleration curves of the joints.
In the embodiment, firstly, the barrier-free path of the working arm is calculated according to the position information of the anchor hole and is dispersed into a plurality of path points; then calculating the movement time of the working arm according to the physical constraint conditions of each joint of the working arm; and finally, calculating the coefficients of the working arm segmented polynomial locus according to the movement time of the working arm between the adjacent path points, and obtaining the working arm segmented polynomial locus. Compared with point-to-point trajectory planning, the method provided by the embodiment can ensure that the working arm passes through the preset path points, and can ensure that the trajectories, speeds and accelerations of the working arm at all the path points are continuous when the physical constraints of the working arm are met, so that the working arm can avoid collision with the external environment under an obstacle-free path, and meanwhile, the movement is continuous and stable.
Example two
As shown in fig. 2, the present embodiment provides a working arm trajectory planning system of a bolting robot, including:
the starting and ending position determining module 101 is used for determining the starting and ending position of the marking joint according to the first anchor hole position and the second anchor hole position; the marking joint is any joint of the working arm.
The starting and ending position determining module specifically comprises:
and the anchor hole position determining submodule is used for acquiring the first anchor hole position and the second anchor hole position.
A starting position calculation submodule for calculating the first anchor eye position based on an inverse kinematics model to determine a starting position of a marker joint; the inverse kinematics model is determined by a kinematics model of the working arm; the kinematic model of the working arm is a six-degree-of-freedom working arm motion model determined by an MD-H parameter method.
And the termination position calculation submodule is used for calculating the second anchor hole position based on the inverse kinematics model so as to determine the termination position of the marking joint.
And an unobstructed track determination module 201, configured to determine an unobstructed travel track of the marked joint according to the starting and ending positions of the marked joint.
A discrete processing module 301, configured to perform discrete processing on the obstacle-free driving trajectory of the marked joint, and determine the number of discrete segments of the obstacle-free driving trajectory of the marked joint and a discrete time corresponding to each discrete segment.
Specifically, in determining the discrete time, the discrete processing module further includes:
according to the formulaCalculating the discrete time corresponding to the n discrete section of the marked joint;
wherein v ismaxSpeed constraint for maximum speed of working arm, PnFor the end path point, P, of the n-th discrete segment of the marked jointn-1Is the starting path point of the nth discrete segment of the marked joint.
The joint driving track determining module 401 is configured to plan a driving path of each remaining joint according to a piecewise polynomial algorithm, the number of discrete segments, and a discrete time corresponding to each discrete segment, and determine a driving track of the remaining joint; the remaining joints are joints in the working arm other than the marked joint.
Specifically, the joint driving trajectory determination module 401 specifically includes:
a driving track calculation submodule for calculating a driving track according to a formula
Planning a driving path of each residual joint and determining a driving track of the residual joint; wherein s isi(t) is the travel locus of the ith remaining joint, sin(t) is the driving track of the ith residual joint in the nth discrete section, tnThe discrete time corresponding to the nth discrete segment of the marked joint is n, which is 1,2, …, k, k is the number of discrete segments; a isi1m、ai2l、…、ai(k-1)l、aikmAre coefficient vectors, m is 0,1,2,3,4, l is 0,1,2, 3.
EXAMPLE III
In this embodiment, a path between two adjacent anchor holes on a roof of the anchor bolt support robot is taken as an example, and the working arm trajectory planning method of the anchor bolt support robot provided in the first embodiment is adopted, where k is 6, that is, a working arm trajectory of a 4-3-3-3-3-4 piecewise polynomial is planned.
Step 1), identifying an anchor hole according to a visual module of the anchor bolt supporting robot, and calculating to obtain a coordinate relative to a working arm base coordinate system of the anchor bolt supporting robot to obtain an anchor hole Q0Relative to the working arm base coordinate system, the Cartesian coordinates are 4000mm, -776mm, 2000mm]The joint coordinates are [ -12.4457 °, 14.2120 °, 443.1127mm, -14.2120 °, 12.4457 °, 0 ° ]]Anchor eye Q6Relative to the working arm base coordinate system, the Cartesian coordinates are 4000mm, -332mm, 2000mm]The joint coordinates are [ -3.9344 °, 14.5603 °, 381.6752mm, -14.5603 °, 3.9344 °, 0 ° ]](ii) a At the same time, the working arm is in the anchor hole Q0And anchor eye Q6Velocity v of0、v6And the working arm is arranged in the anchor hole Q0And anchor eye Q6Acceleration a of0、a6Are all zero.
Step 2), calculating a path point [ Q ] according to the obstacle avoidance requirement of the working arm of the anchor bolt support robot1,Q2,Q3,Q4,Q5]The coordinates of (a).
And 3) solving that the interpolation time t of each piecewise polynomial is 2s according to the physical constraint condition of each joint of the working arm.
And 4) obtaining a motion track of the joint (taking the joint 1 as an example) according to the interpolation time t and a piecewise polynomial formula, wherein a joint motion track diagram of the joint 1 is shown in figure 3, a joint motion velocity diagram of the joint 1 is shown in figure 4, a joint motion acceleration diagram of the joint 1 is shown in figure 5, and further a tail end motion path diagram of the working arm is obtained and is shown in figure 6.
Compared with the prior art, the invention also has the following advantages:
(1) the invention can ensure that the working arm of the anchor bolt supporting robot moves stably without impact at the initial position and the final position, and meanwhile, the anchor bolt supporting robot can complete the anchor bolt supporting task of positioning and finding the hole in the roadway working surface, thereby reducing the possibility of collision.
(2) The invention solves the defects existing in point-to-point track planning, simultaneously ensures the continuous displacement, speed and acceleration of the planned track at each moment, reduces the calculated amount and has smooth track.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.
Claims (10)
1. A working arm track planning method of an anchor bolt support robot is characterized by comprising the following steps:
determining the starting and ending positions of the marking joints according to the first anchor hole position and the second anchor hole position; the marking joint is any joint of the working arm;
determining an obstacle-free running track of the marked joint according to the starting and ending positions of the marked joint;
discrete processing is carried out on the barrier-free running track of the marked joint, and the number of discrete sections of the barrier-free running track of the marked joint and the discrete time corresponding to each discrete section are determined;
planning a driving path of each residual joint according to a segmented polynomial algorithm, the number of the discrete segments and the discrete time corresponding to each discrete segment, and determining a driving track of the residual joint; the remaining joints are any joints of the working arm except the marking joint.
2. The method for planning a working arm trajectory of a bolting robot according to claim 1, wherein the discrete time calculation process specifically comprises:
according to the formulaCalculating the discrete time corresponding to the n discrete section of the marked joint;
wherein v ismaxSpeed constraint for maximum speed of working arm, PnFor the end path point, P, of the n-th discrete segment of the marked jointn-1Is the starting path point of the nth discrete segment of the marked joint.
3. The method for planning the working arm trajectory of the bolting robot according to claim 1, wherein the step of planning the travel path of each remaining joint according to a piecewise polynomial algorithm, the number of discrete segments and the discrete time corresponding to each discrete segment to determine the travel trajectory of the remaining joint comprises:
according to the formula
Planning a driving path of each residual joint and determining a driving track of the residual joint;
wherein s isi(t) is the travel locus of the ith remaining joint, sin(t) is the driving track of the ith residual joint in the nth discrete section, tnThe discrete time corresponding to the nth discrete segment of the marked joint is n, which is 1,2, …, k, k is the number of discrete segments; a isi1m、ai2l、…、ai(k-1)l、aikmAre coefficient vectors, m is 0,1,2,3,4, l is 0,1,2, 3.
4. The method for planning the trajectory of the working arm of the bolting robot according to claim 1, wherein the step of determining the starting and ending positions of the marking joints according to the first anchor hole position and the second anchor hole position comprises the following steps:
acquiring a first anchor hole position and a second anchor hole position;
calculating the first anchor hole position based on an inverse kinematics model to determine a starting position of a marked joint; the inverse kinematics model is determined by a kinematics model of the working arm; the kinematic model of the working arm is a six-degree-of-freedom working arm motion model determined by an MD-H parameter method;
calculating the second anchor eye position based on the inverse kinematics model to determine an end position of a marker joint.
5. The method for planning the trajectory of the working arm of the bolting robot according to claim 1, wherein the step of determining the unobstructed travel trajectory of the marked joint according to the starting and ending positions of the marked joint specifically comprises the steps of:
determining velocity information for the marked joint;
determining an obstacle-free running track of the marked joint according to the starting and ending position and the speed information of the marked joint;
wherein the speed information comprises: the joint velocity of the start position of the marking joint is 0, the joint acceleration of the start position of the marking joint is 0, the joint velocity of the end position of the marking joint is 0, and the joint acceleration of the end position of the marking joint is 0.
6. The working arm trajectory planning method for a bolting robot according to claim 3, wherein the calculation process of the coefficient vector specifically includes:
wherein, wherein n is an element of [2, k-1 ]],Q is an information set; the information set comprises the starting and ending positions of the marked joints, the joint speeds of the starting positions of the marked joints, the joint accelerations of the starting positions of the marked joints, the joint speeds of the ending positions of the marked joints, the joint accelerations of the ending positions of the marked joints and the head and tail positions of the marked joints in each discrete segment.
7. The utility model provides a stock support robot's work arm orbit planning system which characterized in that, work arm orbit planning system includes:
the starting and ending position determining module is used for determining the starting and ending position of the marking joint according to the first anchor hole position and the second anchor hole position; the marking joint is any joint of the working arm;
the barrier-free track determining module is used for determining a barrier-free running track of the marked joint according to the starting and ending positions of the marked joint;
the discrete processing module is used for performing discrete processing on the barrier-free running track of the marked joint and determining the number of discrete sections of the barrier-free running track of the marked joint and the discrete time corresponding to each discrete section;
the joint driving track determining module is used for planning the driving path of each residual joint according to a segmented polynomial algorithm, the number of discrete segments and the discrete time corresponding to each discrete segment, and determining the driving track of the residual joint; the remaining joints are joints in the working arm other than the marked joint.
8. The work arm trajectory planning system of a bolting robot according to claim 7, wherein in determining said discrete time, said discrete processing module further comprises:
according to the formulaCalculating the discrete time corresponding to the n discrete section of the marked joint;
wherein v ismaxSpeed constraint for maximum speed of working arm, PnFor the end path point, P, of the n-th discrete segment of the marked jointn-1Is the starting path point of the nth discrete segment of the marked joint.
9. The working arm trajectory planning system of a bolting robot according to claim 7, wherein said joint travel trajectory determination module specifically comprises:
a driving track calculation submodule for calculating a driving track according to a formula
Planning a driving path of each residual joint and determining a driving track of the residual joint;
wherein s isi(t) is the travel locus of the ith remaining joint, sin(t) is the driving track of the ith residual joint in the nth discrete section, tnThe discrete time corresponding to the nth discrete segment of the marked joint is n, which is 1,2, …, k, k is the number of discrete segments; a isi1m、ai2l、…、ai(k-1)l、aikmAre coefficient vectors, m is 0,1,2,3,4, l is 0,1,2, 3.
10. The working arm trajectory planning system of a bolting robot according to claim 7, wherein said start and end position determining module specifically comprises:
the anchor hole position determining submodule is used for acquiring a first anchor hole position and a second anchor hole position;
a starting position calculation submodule for calculating the first anchor eye position based on an inverse kinematics model to determine a starting position of a marker joint; the inverse kinematics model is determined by a kinematics model of the working arm; the kinematic model of the working arm is a six-degree-of-freedom working arm motion model determined by an MD-H parameter method;
and the termination position calculation submodule is used for calculating the second anchor hole position based on the inverse kinematics model so as to determine the termination position of the marking joint.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111483342.XA CN113954081A (en) | 2021-12-07 | 2021-12-07 | Working arm track planning method and system of anchor rod support robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111483342.XA CN113954081A (en) | 2021-12-07 | 2021-12-07 | Working arm track planning method and system of anchor rod support robot |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113954081A true CN113954081A (en) | 2022-01-21 |
Family
ID=79472886
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111483342.XA Pending CN113954081A (en) | 2021-12-07 | 2021-12-07 | Working arm track planning method and system of anchor rod support robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113954081A (en) |
Cited By (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 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103176409A (en) * | 2013-04-15 | 2013-06-26 | 徐州燕大传动与控制技术有限公司 | Method for fast and accurately realizing concrete pump truck cantilever crane movement locus |
WO2018019111A1 (en) * | 2016-07-29 | 2018-02-01 | 深圳光启合众科技有限公司 | Robot and joint motion control method and device therefor |
US10035266B1 (en) * | 2016-01-18 | 2018-07-31 | X Development Llc | Generating robot trajectories using a real time trajectory generator and a path optimizer |
CN108381555A (en) * | 2018-05-30 | 2018-08-10 | 华南理工大学 | The design method of level controller under a kind of flight Work robot redundancy mechanical arm |
CN109877838A (en) * | 2019-03-25 | 2019-06-14 | 重庆邮电大学 | Time optimal mechanical arm method for planning track based on cuckoo searching algorithm |
CN111002308A (en) * | 2019-12-05 | 2020-04-14 | 南京理工大学 | Industrial mechanical arm path planning method based on segmented multistage polynomial interpolation |
CN111399514A (en) * | 2020-03-30 | 2020-07-10 | 浙江钱江机器人有限公司 | Robot time optimal trajectory planning method |
CN111880573A (en) * | 2020-07-31 | 2020-11-03 | 电子科技大学 | Four-rotor autonomous navigation method based on visual inertial navigation fusion |
CN113276120A (en) * | 2021-05-25 | 2021-08-20 | 中国煤炭科工集团太原研究院有限公司 | Control method and device for mechanical arm movement and computer equipment |
CN113589809A (en) * | 2021-07-26 | 2021-11-02 | 江苏徐工工程机械研究院有限公司 | Obstacle-avoidable excavator working device operation track planning method and device |
-
2021
- 2021-12-07 CN CN202111483342.XA patent/CN113954081A/en active Pending
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103176409A (en) * | 2013-04-15 | 2013-06-26 | 徐州燕大传动与控制技术有限公司 | Method for fast and accurately realizing concrete pump truck cantilever crane movement locus |
US10035266B1 (en) * | 2016-01-18 | 2018-07-31 | X Development Llc | Generating robot trajectories using a real time trajectory generator and a path optimizer |
WO2018019111A1 (en) * | 2016-07-29 | 2018-02-01 | 深圳光启合众科技有限公司 | Robot and joint motion control method and device therefor |
CN108381555A (en) * | 2018-05-30 | 2018-08-10 | 华南理工大学 | The design method of level controller under a kind of flight Work robot redundancy mechanical arm |
CN109877838A (en) * | 2019-03-25 | 2019-06-14 | 重庆邮电大学 | Time optimal mechanical arm method for planning track based on cuckoo searching algorithm |
CN111002308A (en) * | 2019-12-05 | 2020-04-14 | 南京理工大学 | Industrial mechanical arm path planning method based on segmented multistage polynomial interpolation |
CN111399514A (en) * | 2020-03-30 | 2020-07-10 | 浙江钱江机器人有限公司 | Robot time optimal trajectory planning method |
CN111880573A (en) * | 2020-07-31 | 2020-11-03 | 电子科技大学 | Four-rotor autonomous navigation method based on visual inertial navigation fusion |
CN113276120A (en) * | 2021-05-25 | 2021-08-20 | 中国煤炭科工集团太原研究院有限公司 | Control method and device for mechanical arm movement and computer equipment |
CN113589809A (en) * | 2021-07-26 | 2021-11-02 | 江苏徐工工程机械研究院有限公司 | Obstacle-avoidable excavator working device operation track planning method and device |
Non-Patent Citations (2)
Title |
---|
ZHIFU GUO, CHANGBAO ZHENG, XING QI: "Control Strategy Study of Bi-directional Energy", 《2017 12TH IEEE CONFERENCE ON INDUSTRIAL ELECTRONICS AND APPLICATIONS》 * |
张良壮: "智能挖掘机器人轨迹规划与任务决策研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (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 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9129236B2 (en) | Drill hole planning | |
CN114193449B (en) | Working arm track planning method of anchor bolt support robot | |
CN106325280A (en) | Multirobot collision preventing method and system | |
CN107346134A (en) | Unmanned control method and device for underground mining articulated vehicle | |
CN104881025A (en) | Reactive navigation control method for underground mining vehicle | |
CN105415372A (en) | Multi-joint robot track planning method under constraint of safety space | |
CN113954081A (en) | Working arm track planning method and system of anchor rod support robot | |
CN109236313B (en) | Flexible arm heading machine for supporting and excavating tunnels with arbitrary sections through serial-parallel robots | |
CN113832968B (en) | Anchor rod construction positioning method and system | |
CN112943323B (en) | Anchor rod trolley control system | |
CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
CN110513043B (en) | Vertical drilling process deviation correction control method based on automatic guiding drilling tool | |
CN112068543A (en) | Coal mine drilling anchor robot drilling accurate positioning method based on visual calibration | |
WO2023130673A1 (en) | Path planning method for wall-climbing robot | |
CN111396047B (en) | Measuring and positioning system and method for coal face equipment group | |
CN115256414A (en) | Mining drilling robot and coupling operation method of mining drilling robot and geological and roadway models | |
AU2004223685B2 (en) | Method for automatically guiding a mining machine | |
CN115016482A (en) | Improved indoor mobile robot global and local path coordination optimization method | |
CN109993369B (en) | System for acquiring high-position rock stratum movement information of coal mine stope and acquisition and analysis method | |
CN113982662B (en) | Underground intelligent anchoring unit for coal mine and dynamic control method thereof | |
CN107168338A (en) | Inertial guide car air navigation aid and inertial guide car based on millimetre-wave radar | |
CN112947433B (en) | Orchard mobile robot and autonomous navigation method thereof | |
CN112065275B (en) | Tunnel drilling robot system, control method thereof and tunnel boring machine | |
CN112012775A (en) | Crawler-type gantry drilling and anchoring robot | |
CN114715363B (en) | Navigation method and system for submarine stratum space drilling robot and electronic equipment |
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 | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20220121 |