CN115781692A - Mechanical arm path planning method and device, intelligent anchoring machine and storage medium - Google Patents
Mechanical arm path planning method and device, intelligent anchoring machine and storage medium Download PDFInfo
- Publication number
- CN115781692A CN115781692A CN202310023115.1A CN202310023115A CN115781692A CN 115781692 A CN115781692 A CN 115781692A CN 202310023115 A CN202310023115 A CN 202310023115A CN 115781692 A CN115781692 A CN 115781692A
- Authority
- CN
- China
- Prior art keywords
- joint
- mechanical arm
- coordinate system
- calculating
- motion
- 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 82
- 238000004873 anchoring Methods 0.000 title claims abstract description 24
- 230000001133 acceleration Effects 0.000 claims abstract description 41
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 40
- 238000004458 analytical method Methods 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 54
- 230000009466 transformation Effects 0.000 claims description 35
- 238000006073 displacement reaction Methods 0.000 claims description 19
- 238000004364 calculation method Methods 0.000 claims description 18
- 238000009795 derivation Methods 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 5
- 230000001131 transforming effect Effects 0.000 claims description 5
- 230000036461 convulsion Effects 0.000 claims description 2
- 230000006870 function Effects 0.000 description 71
- 230000008569 process Effects 0.000 description 27
- 230000005641 tunneling Effects 0.000 description 9
- 238000004088 simulation Methods 0.000 description 7
- 230000008901 benefit Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 5
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000005553 drilling Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000013101 initial test Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
The application discloses a mechanical arm path planning method and device, an intelligent anchoring machine and a storage medium. The method comprises the following steps: according to the specific structure of the mechanical arm, a coordinate system corresponding to each joint of the mechanical arm is constructed; calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data of each connecting rod and each joint of the mechanical arm; and planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm. The mechanical arm path planning method realizes stable and continuous operation of the mechanical cantilever and improves the motion performance of the mechanical arm joint of the anchor and protection heading machine.
Description
Technical Field
The invention relates to the technical field of intelligent anchoring and protecting tunneling machines, in particular to a method and a device for planning a path of a mechanical arm, an intelligent anchoring and protecting machine and a storage medium.
Background
Along with the improvement of the intelligent requirement of underground comprehensive tunneling equipment, the intelligent anchoring and protecting tunneling machine occupies more and more market share, and the requirements on the working accuracy and the motion stability of a cantilever of the tunneling machine are higher and higher. The track planning is to describe the motion path of the cantilever of the intelligent anchoring and protecting heading machine so that the intelligent anchoring and protecting heading machine can run more stably, and most track planning algorithms mainly utilize combination of parabolas, polynomials and different curves.
The mechanical arm is one of important actuating mechanisms for the movement of the intelligent anchoring and protecting heading machine, and replaces a human arm to complete various operations, such as automatic grabbing of an anchor rod, automatic drilling and anchoring and the like. The labor consumption is reduced to a certain extent, and the working efficiency is improved. On the other hand, it is necessary to improve the control accuracy of the trajectory planning for describing the motion path of the mechanical cantilever. In the process of implementing the present invention, the inventor finds that in the prior art, at least the following technical problems exist, in practical application, the cubic polynomial interpolation algorithm is relatively simple and has a small calculation amount, so that the cubic polynomial interpolation algorithm is widely applied, but in the joint motion process, because the space coordinates of the joint change along with time, sudden changes of angular velocity and angular acceleration cause a certain degree of shaking on the joint, and a certain degree of instability phenomenon is generated when the mechanical arm works.
Disclosure of Invention
In view of the above, the invention provides a method and a device for planning a path of a mechanical arm, an intelligent anchoring and protecting machine and a storage medium, and mainly aims to solve the problem that the existing mechanical arm generates a certain degree of instability during working.
In order to solve the above problem, the present application provides a method for planning a path of a robot arm, including:
according to the specific structure of the mechanical arm, a coordinate system corresponding to each joint of the mechanical arm is constructed;
calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data of each connecting rod and each joint of the mechanical arm;
and planning a joint space path by adopting a seventh polynomial interpolation algorithm based on a preset constraint condition and the coordinate value of each motion track, calculating to obtain a joint angle, a joint angular velocity, a joint angular acceleration and an impact degree corresponding to each joint, and obtaining a motion path planning result of the mechanical arm.
Optionally, the D-H parameter data includes: link length, link torsion angle, link offset and joint angle.
Optionally, the constructing a coordinate system corresponding to each joint of the mechanical arm according to the specific structure of the mechanical arm specifically includes:
setting a reference coordinate system;
determining a Z axis of a D-H coordinate system corresponding to each joint based on the rotation pair and the movement pair of each joint;
determining a common perpendicular line of Z axes of two adjacent joints as an X axis;
and determining a Y axis corresponding to each joint by adopting a right-hand rule method based on the Z axis and the X axis, and constructing and obtaining a coordinate system corresponding to each joint of the mechanical arm.
Optionally, the joint space path planning is performed by using a seventh-order polynomial interpolation algorithm based on the preset constraint condition and the coordinate values of the motion trajectories, and the joint angle, the joint angular velocity, the joint angular acceleration and the impact degree corresponding to each joint are obtained through calculation, which specifically includes:
constructing a target function of a seventh-order polynomial interpolation algorithm based on preset constraint conditions;
and performing operation based on the objective function to obtain joint angular velocity, joint angular acceleration and joint impact degree corresponding to each joint.
Optionally, the constructing a target function of a seventh-order polynomial interpolation algorithm based on the preset constraint condition specifically includes:
determining a displacement function expression of a seventh-order polynomial based on each preset parameter and time variable;
calculating a first derivative function, a second derivative function and a third derivative function corresponding to the displacement function expression containing each preset parameter, and calculating to obtain an angular velocity function expression, an angular acceleration function expression and an impact function expression corresponding to the displacement function expression;
and after constraint conditions are set, constructing and obtaining a target function of the seventh-order polynomial interpolation algorithm based on the displacement function expression, the angular velocity function expression, the angular acceleration function expression and the impact function expression.
Optionally, the performing operation based on the objective function to obtain the joint angular velocity, the joint angular acceleration, and the joint impact degree corresponding to each joint specifically includes:
performing first-order derivation calculation on the objective function to obtain joint angular velocities corresponding to the joints;
performing second-order derivation calculation on the objective function to obtain joint angular acceleration corresponding to each joint;
and performing third-order derivation calculation on the target function to obtain joint impact degrees corresponding to the joints.
Optionally, the calculating, based on each coordinate system and the D-H parameter data corresponding to each link and each joint of the mechanical arm, to obtain the coordinate value of the motion trajectory of each joint by using a forward kinematics analysis method specifically includes:
determining each translation transformation matrix for transforming the connecting rod from the current coordinate system to the next coordinate system;
determining a first rotation matrix rotating around an x axis and a second rotation matrix rotating around a Z axis of the connecting rod when the connecting rod is converted from the current coordinate system to the next coordinate system;
calculating to obtain a total transformation matrix based on each translation transformation matrix and the first rotation matrix and the second rotation matrix;
and calculating to obtain the coordinate value of the motion track of each joint based on the transposed transformation matrix of the tail end node of the mechanical arm relative to the reference coordinate system and the total transformation matrix.
In order to solve the above problem, the present application provides a robot path planning apparatus, including: a mechanical arm and a control module;
the mechanical arm is used for anchoring and protecting operation;
the control module is used for constructing a coordinate system corresponding to each joint of the mechanical arm according to the specific structure of the mechanical arm; calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data of each connecting rod and each joint of the mechanical arm; and planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm.
In order to solve the problem, the application provides an intelligence anchor protects machine, including the entry driving machine and as above the anchor protect the device, the anchor protect the device be provided with at least one set, at least one set the anchor protect the device set up in on the entry driving machine.
In order to solve the above problem, the present application provides a storage medium, where a computer program is stored, and the computer program, when executed by a processor, implements the steps of the robot path planning method.
According to the method, a coordinate system corresponding to each joint of the mechanical arm is established according to the specific structure of the mechanical arm; calculating to obtain the coordinate value of the motion track of each joint by adopting a forward kinematics analysis method based on each coordinate system and the D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm; and planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm. The mechanical arm path planning method realizes stable and continuous operation of the mechanical cantilever and improves the motion performance of the mechanical arm joint of the anchor and protection heading machine.
The above description is only an overview of the technical solutions of the present invention, and the present invention can be implemented in accordance with the content of the description so as to make the technical means of the present invention more clearly understood, and the above and other objects, features, and advantages of the present invention will be more clearly understood.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are only for purposes of illustrating the preferred embodiments and are not to be construed as limiting the invention. Also, like reference numerals are used to refer to like parts throughout the drawings. In the drawings:
fig. 1 is a flowchart of a method for planning a path of a robot arm according to an embodiment of the present disclosure;
FIG. 2 is a flow chart of another method for planning a path of a robotic arm according to yet another embodiment of the present disclosure;
fig. 3 is a block diagram of a robot path planning apparatus according to yet another embodiment of the present disclosure;
FIG. 4 is a schematic diagram of a coordinate system corresponding to each joint of a robotic arm according to one embodiment provided herein;
fig. 5 is a graph of angular velocity of a joint, angular acceleration of a joint, and an impact degree of a joint at each time obtained by performing path planning using a seven-term polynomial interpolation planning algorithm according to an embodiment of the present disclosure.
Detailed Description
Various aspects and features of the present application are described herein with reference to the accompanying drawings.
It will be understood that various modifications may be made to the embodiments of the present application. Accordingly, the foregoing description should not be considered as limiting, but merely as exemplifications of embodiments. Those skilled in the art will envision other modifications within the scope and spirit of the application.
The accompanying drawings, which are incorporated in and constitute a part of the specification, illustrate embodiments of the application and, together with a general description of the application given above and the detailed description of the embodiments given below, serve to explain the principles of the application.
These and other characteristics of the present application will become apparent from the following description of a preferred form of embodiment, given as a non-limiting example, with reference to the attached drawings.
It is also to be understood that although the present application has been described with reference to some specific examples, those skilled in the art are able to ascertain many other equivalents to the practice of the present application.
The above and other aspects, features and advantages of the present application will become more apparent in view of the following detailed description when taken in conjunction with the accompanying drawings.
Specific embodiments of the present application are described hereinafter with reference to the accompanying drawings; however, it is to be understood that the disclosed embodiments are merely exemplary of the application, which can be embodied in various forms. Well-known and/or repeated functions and constructions are not described in detail to avoid obscuring the application of unnecessary or unnecessary detail. Therefore, specific structural and functional details disclosed herein are not to be interpreted as limiting, but merely as a basis for the claims and as a representative basis for teaching one skilled in the art to variously employ the present application in virtually any appropriately detailed structure.
The specification may use the phrases "in one embodiment," "in another embodiment," "in yet another embodiment," or "in other embodiments," which may each refer to one or more of the same or different embodiments in accordance with the application.
The embodiment of the application provides a method for planning a path of a mechanical arm, as shown in fig. 1, the method includes:
step S101: according to the specific structure of the mechanical arm, a coordinate system corresponding to each joint of the mechanical arm is constructed;
in the specific implementation process, in the application scene of anchoring and protecting the tunneling machine, a base of a mechanical arm of the tunneling machine is set as a reference coordinate system, and each joint of the mechanical arm is respectively provided with a coordinate system; specifically, the Z axis of a coordinate system corresponding to each joint is determined based on the revolute pair and the revolute pair of each joint; determining a common perpendicular line of Z axes of two adjacent joints as an X axis; and determining a Y axis corresponding to each joint by adopting a right-hand rule method based on the Z axis and the X axis, and constructing and obtaining a coordinate system corresponding to each joint of the mechanical arm. Assuming that the robot arm is a five-degree-of-freedom robot arm, a coordinate system as shown in fig. 4 can be created, and the present application can accommodate robot arms of various degrees of freedom without limitation to the number of degrees of freedom.
Step S102: calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm;
in the specific implementation process of the step, the data of the D-H parameters (Denavit-Hartenberg parameters, D-H parameters for short) comprises the following steps: the length of the connecting rod, the torsion angle of the connecting rod, the offset distance of the connecting rod and the joint angle; specifically, with reference to fig. 4, the parameter data includes:
the parameter is expressed along
A shaft from
Move to
The length of the connecting rod; parameter(s)
Represent along
A shaft from
Is rotated to
The connecting rod torsion angle of (1); parameter(s)
Represent along
A shaft from
Move to
The link offset of (2); parameter(s)
Represent along
A shaft from
Is rotated to
The joint angle of (a). The specific process of calculating and obtaining the motion track coordinate value of each joint by adopting the forward kinematics analysis method comprises the following steps: determining each translation transformation matrix for transforming the connecting rod from the current coordinate system to the next coordinate system; determining a first rotation matrix rotating around an x axis and a second rotation matrix rotating around a Z axis of the connecting rod from the current coordinate system to the next coordinate system; calculating to obtain a total transformation matrix based on each translation transformation matrix and the first rotation matrix and the second rotation matrix; and calculating to obtain the coordinate value of the motion track of each joint based on the transposed transformation matrix of the tail end node of the mechanical arm relative to the reference coordinate system and the total transformation matrix. Kinematic analysis is a prerequisite for trajectory planning for a mechanical arm. The forward kinematics is the known geometric parameters of the mechanical arm, the terminal pose of the mechanical arm is obtained, and the motion of the mechanical arm can be regarded as the translation and rotation motion of a connecting rod of a rigid body. The general transformation matrix and the transposed transformation matrix are combined, a general formula for pose transformation between two adjacent connecting rods can be calculated, and then the coordinate value of the motion track of each joint can be obtained.
Step S103: and planning a joint space path by adopting a seventh polynomial interpolation algorithm based on a preset constraint condition and the coordinate value of each motion track, calculating to obtain a joint angle, a joint angular velocity, a joint angular acceleration and an impact degree corresponding to each joint, and obtaining a motion path planning result of the mechanical arm.
In the specific implementation process of the step, firstly, a target function of a seventh-order polynomial interpolation algorithm is constructed based on preset constraint conditions; then, calculation is performed based on the objective function to obtain joint angular velocity, joint angular acceleration, and joint jerk corresponding to each joint. Under the simulation environment, each joint angle can be solved by utilizing an ikine function in MATLAB software in preset simulation time and the coordinate values of an initial point and an end point of the mechanical arm motion, the process from two points is simulated by adopting a locus planning algorithm of seventh polynomial interpolation, and the curve of the angular velocity, the angular acceleration and the impact of each joint of the mechanical arm continuously does not have sudden change along with the time change, so that the stable and continuous operation of the mechanical cantilever is realized, and the motion performance of the mechanical arm joint of the anchor and shield tunneling machine is improved.
According to the method, a coordinate system corresponding to each joint of the mechanical arm is established according to the specific structure of the mechanical arm; calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm; and planning a joint space path by adopting a seventh polynomial interpolation algorithm based on a preset constraint condition and the coordinate value of each motion track, calculating to obtain a joint angle, a joint angular velocity, a joint angular acceleration and an impact degree corresponding to each joint, and obtaining a motion path planning result of the mechanical arm. The mechanical arm path planning method realizes stable and continuous operation of the mechanical cantilever and improves the motion performance of the mechanical arm joint of the anchor and protection heading machine.
Another embodiment of the present application provides a method for planning a path of a robot arm, as shown in fig. 2, including:
step S201: according to the specific structure of the mechanical arm, a coordinate system corresponding to each joint of the mechanical arm is constructed;
in the specific implementation process of the step, in order to describe the relative motion relation of each joint of the mechanical arm of the intelligent anchoring and protecting heading machine, a D-H parameter method proposed by Denavit and Hartenberg is generally adopted as a modeling method. The D-H parametric method describes the pose and relative position of these coordinate systems by homogeneous transformations. The homogeneous transformation matrix is obtained in a recursive manner, which relates the spatial displacement of the cantilever coordinate system to the reference coordinate system, representing the pose of the end effector relative to the fixed reference coordinate. Specifically, a reference coordinate system is set; in the implementation process, the position of the mechanical arm base can be used as a reference coordinate system. Determining a Z axis of a D-H coordinate system corresponding to each joint based on the rotation pair and the movement pair of each joint; determining a common perpendicular line of Z axes of two adjacent joints as an X axis; and determining a Y axis corresponding to each joint by adopting a right-hand rule method based on the Z axis and the X axis, and constructing and obtaining a D-H coordinate system corresponding to each joint of the mechanical arm. In the mechanical arm and the space mechanism, the main function of the connecting rod is to keep the joint axes at the two ends of the connecting rod to have a fixed geometrical relationship, the characteristics of the connecting rod can also be specified by the two axes, and in order to obtain the displacement relationship between the connecting rods of the mechanical arm, a coordinate system can be fixed at the joint position of each connecting rod, and the motion path analysis of the mechanical arm can be carried out by means of the coordinate systems. As shown in fig. 4, a coordinate system corresponding to each joint of the robot arm with 5 degrees of freedom established using the base as a reference coordinate system is shown. The D-H parameter method is the most commonly used method for analyzing the kinematics of the cantilever of the intelligent anchoring and protecting heading machine at present, and has the advantages that no matter how many degrees of freedom the mechanical cantilever has, the structure is complicated, and the method can represent the pose of the mechanical cantilever as long as the method can be simplified into a link mechanism, so that a mathematical model of the kinematics is established.
Step S202: calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm;
in the specific implementation process of the step, after a D-H coordinate system is established, D-H is introduced when describing the motion relation between the connecting rod and the previous connecting rodD-H parameter data of a parametric method, the D-H parameter data comprising: the length of the connecting rod, the torsion angle of the connecting rod, the offset distance of the connecting rod and the joint angle; specifically, with reference to fig. 4, the parameter data includes:
the parameter is expressed along
A shaft from
Move
The length of the connecting rod; parameter(s)
Represent along
A shaft from
Is rotated to
The connecting rod torsion angle of (1); parameter(s)
Represent along
A shaft from
Move to
The link offset of (2); parameter(s)
Represent along
A shaft from
Is rotated to
The joint angle of (a). Specifically, the specific process of calculating and obtaining the motion trajectory coordinate value of each joint by using the forward kinematics analysis method is as follows: determining each translation transformation matrix for transforming the connecting rod from the current coordinate system to the next coordinate system; the translation transformation matrices are Trans (0, dn + 1) and Trans (an +1, 0); then, determining a first rotation matrix rotating around an X axis and a second rotation matrix rotating around a Z axis, wherein the connecting rod is converted from the current coordinate system to the next coordinate system; specifically, the first rotation matrix is
(ii) a The second rotation matrix is
(ii) a Calculating to obtain a total transformation matrix based on each translation transformation matrix and the first rotation matrix and the second rotation matrix; the overall transformation matrix may be as shown in equation (1) below:
and finally, calculating to obtain the coordinate value of the motion track of each joint based on the transposed transformation matrix of the tail end node of the mechanical arm relative to the reference coordinate system and the total transformation matrix. Specifically, for a mechanical arm with n degrees of freedom, the coordinates of the end of the mechanical arm relative to the fixed base coordinate system can be transformed by using a transpose transformation matrix as shown in the following formula (2):
(ii) a In practical applications, the transposed transform matrix is equal to the total transform matrix, i.e.:
(ii) a Therefore, a general formula for pose transformation between two adjacent connecting rods can be obtained by combining the formula (1) and the formula (2): as shown in the following equation (3):
and calculating to obtain the coordinate value of the motion track of each joint based on each coordinate system, the D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm and the general formula for pose transformation between two adjacent connecting rods.
Step S203: determining a displacement function expression of a seventh-order polynomial based on each preset parameter and time variable;
in the specific implementation process of the step, a polynomial interpolation algorithm is a widely applied method in trajectory planning of joint space, wherein a cubic polynomial interpolation algorithm is a basic planning method which is relatively simple in calculation and limited by constraint conditions and unknowns, the obtained angular acceleration mutation of the joint variable is obvious, the mechanical arm shakes greatly in the actual operation process from an initial test point to a target point, and the stability is poor. Therefore, the method and the device plan the path of the mechanical arm by adopting a seventh polynomial interpolation algorithm to solve the problems of large jitter and poor stability. The motion trajectory curve of the seventh polynomial interpolation algorithm is smoother, the motion process is more stable, and the accuracy is higher than that of the third polynomial interpolation algorithm. The displacement expression of the seventh-order polynomial interpolation algorithm is shown in the following formula (4):
wherein: theta (t) is the angle of the joint,
is a coefficient of a polynomial; t is a time variable.
Step S204: calculating a first derivative function, a second derivative function and a third derivative function corresponding to the displacement function expression containing each preset parameter, and calculating to obtain an angular velocity function expression, an angular acceleration function expression and an impact function expression corresponding to the displacement function expression;
in the specific implementation process of the step, the angular velocity function expression, the angular acceleration function expression and the impact function expression are shown as the following formula (5):
step S205: after constraint conditions are set, constructing and obtaining a target function of the seventh polynomial interpolation algorithm based on the displacement function expression, the angular velocity function expression, the angular acceleration function expression and the impact function expression;
in the specific implementation process of the step, the initial time is set as
At the end point of time of
(ii) a The remaining constraints are as shown in equation (6) below:
calculating based on the formula (4), the formula (5) and the formula (6), and constructing and obtaining an objective function of the seven-degree polynomial interpolation algorithm, wherein the objective function is as shown in the following formula (7):
step S206: and calculating based on the objective function to obtain joint angular velocity, joint angular acceleration and joint impact degree corresponding to each joint to obtain a motion path planning result of the mechanical arm.
In the specific implementation process, first-order derivation calculation is carried out on the objective function to obtain joint angular velocities corresponding to the joints; performing second-order derivation calculation on the objective function to obtain joint angular acceleration corresponding to each joint; and performing third-order derivation calculation on the target function to obtain joint impact degrees corresponding to the joints. As shown in the following equation (8):
under a simulation environment, each joint angle can be calculated by utilizing an ikine function in MATLAB software at preset simulation time and the coordinate values of an initial point and an end point of the movement of the mechanical arm, under the simulation environment, the simulation time t =3s is set, the space coordinate position of the mechanical arm of the tunneling machine moving from one point to another point is known, the movement process is simulated by adopting a seven-degree polynomial interpolation trajectory planning algorithm, the simulation result is shown in figure 5, the movement process of the two points is simulated by adopting the seven-degree polynomial interpolation trajectory planning algorithm, the angular velocity, the angular acceleration and the impact of each joint of the mechanical arm are continuous and have no sudden change along with a time change curve, the stable and continuous operation of the mechanical cantilever is realized, and the movement performance of the mechanical arm joints of the anchoring and protecting tunneling machine is improved.
According to the method, a coordinate system corresponding to each joint of the mechanical arm is established according to the specific structure of the mechanical arm; calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm; and planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm. The mechanical arm path planning method realizes stable and continuous operation of the mechanical cantilever and improves the motion performance of the mechanical arm joint of the anchor and protection heading machine.
Another embodiment of the present application provides a robot path planning apparatus, as shown in fig. 3, including: the mechanical arm 1 and the control module 2;
the mechanical arm 1 is used for anchoring and protecting operation;
the control module 2 comprises a construction unit, a calculation unit and a path planning unit;
the construction unit is used for constructing a coordinate system corresponding to each joint of the mechanical arm according to the specific structure of the mechanical arm; the computing unit is used for computing and obtaining the coordinate value of the motion track of each joint by adopting a forward kinematics analysis method based on each coordinate system and the D-H parameter data of each connecting rod and each joint of the mechanical arm; the path planning unit is used for planning a joint space path by adopting a seventh-order polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, calculating to obtain a joint angle, a joint angular velocity, a joint angular acceleration and an impact degree corresponding to each joint, and obtaining a motion path planning result of the mechanical arm.
In a specific implementation process, the control module 2 is further configured to: setting a reference coordinate system; determining the Z axis of a D-H coordinate system corresponding to each joint based on the revolute pair and the revolute pair of each joint; determining a common perpendicular line of Z axes of two adjacent joints as an X axis; and determining a Y axis corresponding to each joint by adopting a right-hand rule method based on the Z axis and the X axis, and constructing and obtaining a coordinate system corresponding to each joint of the mechanical arm.
In a specific implementation process, the control module 2 is further configured to: constructing a target function of a seventh-order polynomial interpolation algorithm based on preset constraint conditions; and performing operation based on the objective function to obtain joint angular velocity, joint angular acceleration and joint impact degree corresponding to each joint.
In a specific implementation process, the control module 2 is further configured to: determining a displacement function expression of a seventh-order polynomial based on each preset parameter and time variable; calculating a first derivative function, a second derivative function and a third derivative function corresponding to the displacement function expression containing each preset parameter, and calculating to obtain an angular velocity function expression, an angular acceleration function expression and an impact function expression corresponding to the displacement function expression; and after constraint conditions are set, constructing and obtaining a target function of the seventh-order polynomial interpolation algorithm based on the displacement function expression, the angular velocity function expression, the angular acceleration function expression and the impact function expression.
In a specific implementation process, the path control module 2 is further configured to perform first-order derivation calculation on the objective function to obtain joint angular velocities corresponding to the joints; performing second-order derivation calculation on the objective function to obtain joint angular acceleration corresponding to each joint; and performing third-order derivation calculation on the target function to obtain joint impact degrees corresponding to the joints.
In the specific implementation process, the control module 2 is also used for determining each translation transformation matrix for transforming the connecting rod from the current coordinate system to the next coordinate system; determining a first rotation matrix rotating around an x axis and a second rotation matrix rotating around a Z axis of the connecting rod from the current coordinate system to the next coordinate system; calculating to obtain a total transformation matrix based on each translation transformation matrix and the first rotation matrix and the second rotation matrix; and calculating to obtain the coordinate value of the motion track of each joint based on the transposed transformation matrix of the tail end node of the mechanical arm relative to the reference coordinate system and the total transformation matrix. The calculation module is configured with D-H parameter data, the D-H parameter data comprising: link length, link torsion angle, link offset and joint angle.
According to the method, a coordinate system corresponding to each joint of the mechanical arm is established according to the specific structure of the mechanical arm; calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm; and planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm. The mechanical arm path planning method realizes stable and continuous operation of the mechanical cantilever and improves the motion performance of the mechanical arm joint of the anchor and protection heading machine.
Another embodiment of this application provides an intelligence anchor protects machine, includes the entry driving machine and as above the anchor protects the device, the anchor protects the device and is provided with at least one set, at least one set the anchor protects the device set up in on the entry driving machine. The anchoring device in this embodiment may be the anchoring device in any of the above embodiments, and is not described herein again.
According to the method, a coordinate system corresponding to each joint of the mechanical arm is established according to the specific structure of the mechanical arm; calculating to obtain the coordinate value of the motion track of each joint by adopting a forward kinematics analysis method based on each coordinate system and the D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm; and planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm. The mechanical arm path planning method realizes stable and continuous operation of the mechanical cantilever and improves the motion performance of the mechanical arm joint of the anchor and protection heading machine.
Another embodiment of the present application provides a storage medium storing a computer program which, when executed by a processor, performs the method steps of:
firstly, constructing a coordinate system corresponding to each joint of the mechanical arm according to the specific structure of the mechanical arm;
secondly, calculating to obtain the coordinate value of the motion track of each joint by adopting a forward kinematics analysis method based on each coordinate system and the D-H parameter data of each connecting rod and each joint of the mechanical arm;
and thirdly, planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm.
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by hardware instructions of a computer program, which can be stored in a non-volatile computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in the embodiments provided herein may include non-volatile and/or volatile memory, among others. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically Programmable ROM (EPROM), electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double Data Rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous Link DRAM (SLDRAM), rambus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).
It should be clear to those skilled in the art that, for convenience and simplicity of description, the foregoing division of the functional units and modules is only used for illustration, and in practical applications, the above function distribution may be performed by different functional units and modules as needed, that is, the internal structure of the apparatus may be divided into different functional units or modules to perform all or part of the above described functions.
The specific implementation process of the above method steps can be referred to any embodiment of the mechanical arm path planning method, and the detailed description of this embodiment is not repeated here.
According to the method, a coordinate system corresponding to each joint of the mechanical arm is established according to the specific structure of the mechanical arm; calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data corresponding to each connecting rod and each joint of the mechanical arm; and planning a joint space path by adopting a seventh polynomial interpolation algorithm based on a preset constraint condition and the coordinate value of each motion track, calculating to obtain a joint angle, a joint angular velocity, a joint angular acceleration and an impact degree corresponding to each joint, and obtaining a motion path planning result of the mechanical arm. The mechanical arm path planning method realizes stable and continuous operation of the mechanical cantilever and improves the movement performance of the mechanical arm joint of the anchor and protection heading machine.
The above embodiments are only exemplary embodiments of the present application, and are not intended to limit the present application, and the protection scope of the present application is defined by the claims. Various modifications and equivalents may be made by those skilled in the art within the spirit and scope of the present application and such modifications and equivalents should also be considered to be within the scope of the present application.
Claims (10)
1. A method for planning a path of a mechanical arm is characterized by comprising the following steps:
according to the specific structure of the mechanical arm, a coordinate system corresponding to each joint of the mechanical arm is constructed;
calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data of each connecting rod and each joint of the mechanical arm;
and planning a joint space path by adopting a seventh polynomial interpolation algorithm based on a preset constraint condition and the coordinate value of each motion track, calculating to obtain a joint angle, a joint angular velocity, a joint angular acceleration and an impact degree corresponding to each joint, and obtaining a motion path planning result of the mechanical arm.
2. The method of claim 1, wherein the D-H parameter data comprises: link length, link torsion angle, link offset and joint angle.
3. The method according to claim 1, wherein the constructing a coordinate system corresponding to each joint of the robot arm according to the specific structure of the robot arm specifically comprises:
setting a reference coordinate system;
determining the Z axis of a D-H coordinate system corresponding to each joint based on the revolute pair and the revolute pair of each joint;
determining a common perpendicular line of Z axes of two adjacent joints as an X axis;
and determining a Y axis corresponding to each joint by adopting a right-hand rule method based on the Z axis and the X axis, and constructing to obtain a coordinate system corresponding to each joint of the mechanical arm.
4. The method of claim 3, wherein the step of performing joint space path planning by using a seventh-order polynomial interpolation algorithm based on the preset constraint condition and the coordinate values of each motion trajectory to calculate and obtain a joint angle, a joint angular velocity, a joint angular acceleration and an impact degree corresponding to each joint specifically comprises:
constructing a target function of a seventh polynomial interpolation algorithm based on a preset constraint condition;
and performing operation based on the objective function to obtain joint angular velocity, joint angular acceleration and joint impact degree corresponding to each joint.
5. The method of claim 4, wherein the constructing the objective function of the seventh order polynomial interpolation algorithm based on the preset constraint condition specifically comprises:
determining a displacement function expression of a seventh-order polynomial based on each preset parameter and time variable;
calculating a first derivative function, a second derivative function and a third derivative function corresponding to the displacement function expression containing each preset parameter, and calculating to obtain an angular velocity function expression, an angular acceleration function expression and an impact function expression corresponding to the displacement function expression;
and after constraint conditions are set, constructing and obtaining a target function of the seventh-order polynomial interpolation algorithm based on the displacement function expression, the angular velocity function expression, the angular acceleration function expression and the impact function expression.
6. The method according to claim 4, wherein the performing the operation based on the objective function to obtain the joint angular velocity, the joint angular acceleration, and the joint jerk corresponding to each of the joints comprises:
performing first-order derivation calculation on the objective function to obtain joint angular velocities corresponding to the joints;
performing second-order derivation calculation on the objective function to obtain joint angular acceleration corresponding to each joint;
and performing third-order derivation calculation on the target function to obtain joint impact degrees corresponding to the joints.
7. The method according to claim 1, wherein the calculating to obtain the coordinate value of the motion trajectory of each joint by using a forward kinematics analysis method based on each coordinate system and the D-H parameter data corresponding to each link and each joint of the robot arm specifically comprises:
determining each translation transformation matrix for transforming the connecting rod from the current coordinate system to the next coordinate system;
determining a first rotation matrix rotating around an x axis and a second rotation matrix rotating around a Z axis of the connecting rod when the connecting rod is converted from the current coordinate system to the next coordinate system;
calculating to obtain a total transformation matrix based on each translation transformation matrix and the first rotation matrix and the second rotation matrix;
and calculating to obtain the coordinate value of the motion track of each joint based on the transposed transformation matrix of the tail end node of the mechanical arm relative to the reference coordinate system and the total transformation matrix.
8. An anchoring device, comprising: a mechanical arm and a control module;
the mechanical arm is used for anchoring and protecting operation;
the control module is used for constructing a coordinate system corresponding to each joint of the mechanical arm according to the specific structure of the mechanical arm; calculating to obtain a motion track coordinate value of each joint by adopting a forward kinematics analysis method based on each coordinate system and D-H parameter data of each connecting rod and each joint of the mechanical arm; and planning joint space paths by adopting a seventh polynomial interpolation algorithm based on preset constraint conditions and the coordinate values of the motion tracks, and calculating to obtain joint angles, joint angular velocities, joint angular accelerations and impact degrees corresponding to the joints to obtain a motion path planning result of the mechanical arm.
9. An intelligent anchoring and protecting machine, which is characterized by comprising a heading machine and the anchoring and protecting device according to claim 8, wherein at least one set of the anchoring and protecting device is arranged on the heading machine.
10. A storage medium, characterized in that the storage medium stores a computer program which, when executed by a processor, carries out the steps of the robot path planning method of any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310023115.1A CN115781692A (en) | 2023-01-09 | 2023-01-09 | Mechanical arm path planning method and device, intelligent anchoring machine and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310023115.1A CN115781692A (en) | 2023-01-09 | 2023-01-09 | Mechanical arm path planning method and device, intelligent anchoring machine and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115781692A true CN115781692A (en) | 2023-03-14 |
Family
ID=85428739
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310023115.1A Pending CN115781692A (en) | 2023-01-09 | 2023-01-09 | Mechanical arm path planning method and device, intelligent anchoring machine and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115781692A (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014161917A (en) * | 2013-02-21 | 2014-09-08 | Seiko Epson Corp | Robot control system, robot, robot control method, and program |
| CN111002308A (en) * | 2019-12-05 | 2020-04-14 | 南京理工大学 | Industrial mechanical arm path planning method based on segmented multistage polynomial interpolation |
| CN112405519A (en) * | 2019-08-23 | 2021-02-26 | 贵州大学 | Motion trajectory planning method for loading and unloading robot |
| CN113276120A (en) * | 2021-05-25 | 2021-08-20 | 中国煤炭科工集团太原研究院有限公司 | Control method and device for mechanical arm movement and computer equipment |
| CN113982690A (en) * | 2021-10-26 | 2022-01-28 | 三一智矿科技有限公司 | Control method, device and system based on anchoring and protecting operation and tunneling, anchoring and protecting all-in-one machine |
| CN114193449A (en) * | 2021-12-09 | 2022-03-18 | 中国煤炭科工集团太原研究院有限公司 | Anchor rod support robot working arm track planning method |
| CN115110974A (en) * | 2022-06-22 | 2022-09-27 | 精英数智科技股份有限公司 | Anchor protection operation monitoring method, device, server, system and readable storage medium |
-
2023
- 2023-01-09 CN CN202310023115.1A patent/CN115781692A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014161917A (en) * | 2013-02-21 | 2014-09-08 | Seiko Epson Corp | Robot control system, robot, robot control method, and program |
| CN112405519A (en) * | 2019-08-23 | 2021-02-26 | 贵州大学 | Motion trajectory planning method for loading and unloading robot |
| CN111002308A (en) * | 2019-12-05 | 2020-04-14 | 南京理工大学 | Industrial mechanical arm path planning method based on segmented multistage polynomial interpolation |
| CN113276120A (en) * | 2021-05-25 | 2021-08-20 | 中国煤炭科工集团太原研究院有限公司 | Control method and device for mechanical arm movement and computer equipment |
| CN113982690A (en) * | 2021-10-26 | 2022-01-28 | 三一智矿科技有限公司 | Control method, device and system based on anchoring and protecting operation and tunneling, anchoring and protecting all-in-one machine |
| CN114193449A (en) * | 2021-12-09 | 2022-03-18 | 中国煤炭科工集团太原研究院有限公司 | Anchor rod support robot working arm track planning method |
| CN115110974A (en) * | 2022-06-22 | 2022-09-27 | 精英数智科技股份有限公司 | Anchor protection operation monitoring method, device, server, system and readable storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 张蕾等: "四自由度机器人抓取运动目标建模与轨迹规划" * |
| 蒋志宏等, 北京理工大学出版社 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108356819B (en) | Industrial mechanical arm collision-free path planning method based on improved A-x algorithm | |
| CN112677153B (en) | Improved RRT algorithm and industrial robot path obstacle avoidance planning method | |
| US8600554B2 (en) | System and method for robot trajectory generation with continuous accelerations | |
| Kalani et al. | Improved general solution for the dynamic modeling of Gough–Stewart platform based on principle of virtual work | |
| CN109397271B (en) | 7-degree-of-freedom anthropomorphic mechanical arm and control method and system thereof | |
| US8560122B2 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
| CN113352319B (en) | Redundant mechanical arm obstacle avoidance trajectory planning method based on improved fast expansion random tree | |
| CN111347429A (en) | Collision detection mechanical arm path planning method based on improved ant colony algorithm | |
| CN111923039B (en) | Redundant mechanical arm path planning method based on reinforcement learning | |
| CN108638055B (en) | Autonomous obstacle avoidance planning method for seven-degree-of-freedom space manipulator | |
| CN113618742B (en) | Robot obstacle avoidance method and device and robot | |
| CN109344477B (en) | 6-degree-of-freedom mechanical arm inverse kinematics solving method | |
| CN116038702B (en) | Seven-axis robot inverse solution method and seven-axis robot | |
| CN113276121B (en) | Redundant manipulator moving obstacle avoiding method based on quadratic programming | |
| CN111914980A (en) | Redundant mechanical arm kinematics resolving method based on analytical solution and improved genetic algorithm | |
| CN115213905B (en) | Method and system for controlling position and pose of redundant mechanical arm and robot | |
| CN111230860B (en) | Robot control method, robot control device, computer device, and storage medium | |
| CN115781692A (en) | Mechanical arm path planning method and device, intelligent anchoring machine and storage medium | |
| CN116038686B (en) | Robot singular point avoidance method, apparatus, computer device, and storage medium | |
| CN116237950A (en) | Robot tail end accurate control method and equipment based on sectional motion planning strategy | |
| CN112975954B (en) | Control method of robot arm, computer device, and storage medium | |
| CN116141330A (en) | Robot motion control method and device, robot equipment and storage medium | |
| CN112917479B (en) | Approximate pose calculation method and device of five-axis robot and storage medium | |
| CN114851168A (en) | Motion planning and control method and system of joint-limited redundant parallel mechanical arm and robot | |
| CN114734435A (en) | Encoder calibration method, device and system based on hypersphere |
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 |
Application publication date: 20230314 |
|
| RJ01 | Rejection of invention patent application after publication |