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 PDF

Info

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
Application number
CN202111483342.XA
Other languages
Chinese (zh)
Inventor
邓镓敏
高志强
郭治富
阎志伟
金江
刘伟立
赵永红
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Coal Research Institute CCRI
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Original Assignee
China Coal Research Institute CCRI
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by China Coal Research Institute CCRI, Taiyuan Institute of China Coal Technology and Engineering Group, Shanxi Tiandi Coal Mining Machinery Co Ltd filed Critical China Coal Research Institute CCRI
Priority to CN202111483342.XA priority Critical patent/CN113954081A/en
Publication of CN113954081A publication Critical patent/CN113954081A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1656Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664Programme 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

Working arm track planning method and system of anchor rod support robot
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 formula
Figure BDA0003396334370000021
Calculating 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
Figure BDA0003396334370000022
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:
according to the formula
Figure BDA0003396334370000031
Calculating a coefficient vector;
wherein,
Figure BDA0003396334370000032
Figure BDA0003396334370000033
Figure BDA0003396334370000034
Figure BDA0003396334370000035
wherein n is an element of [2, k-1 ]],
Figure BDA0003396334370000036
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 formula
Figure BDA0003396334370000041
Calculating 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
Figure BDA0003396334370000042
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 100, determining the starting and ending positions of the marked joints according to the first anchor hole position and the second anchor hole position; the marking joint is any joint of the working arm. Specifically, step 100 specifically 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 is
Figure BDA0003396334370000061
Wherein,
Figure BDA0003396334370000062
is a posture matrix of the drill frame at the tail end of the working arm,
Figure BDA0003396334370000063
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 formula
Figure BDA0003396334370000071
And 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.
Step 400, planning a driving path of each residual joint according to a piecewise polynomial algorithm, the number of 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. Specifically, the step 400 specifically includes: according to the formula
Figure BDA0003396334370000081
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:
according to the formula
Figure BDA0003396334370000082
Calculating a coefficient vector;
wherein,
Figure BDA0003396334370000083
Figure BDA0003396334370000084
Figure BDA0003396334370000085
Figure BDA0003396334370000086
wherein n is an element of [2, k-1 ]],
Figure BDA0003396334370000091
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
Figure BDA0003396334370000092
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 formula
Figure BDA0003396334370000101
Calculating 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
Figure BDA0003396334370000102
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 formula
Figure FDA0003396334360000011
Calculating 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
Figure FDA0003396334360000012
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:
according to the formula
Figure FDA0003396334360000021
Calculating a coefficient vector;
wherein,
Figure FDA0003396334360000031
Figure FDA0003396334360000032
Figure FDA0003396334360000033
wherein n is an element of [2, k-1 ]],
Figure FDA0003396334360000034
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 formula
Figure FDA0003396334360000041
Calculating 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
Figure FDA0003396334360000042
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.
CN202111483342.XA 2021-12-07 2021-12-07 Working arm track planning method and system of anchor rod support robot Pending CN113954081A (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (10)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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