CN114633258B - Planning method and related device for movement track of mechanical arm in tunnel environment - Google Patents
Planning method and related device for movement track of mechanical arm in tunnel environment Download PDFInfo
- Publication number
- CN114633258B CN114633258B CN202210433946.1A CN202210433946A CN114633258B CN 114633258 B CN114633258 B CN 114633258B CN 202210433946 A CN202210433946 A CN 202210433946A CN 114633258 B CN114633258 B CN 114633258B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- collision
- planning
- collision detection
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
Abstract
The application discloses a planning method of a mechanical arm movement track in a tunnel environment, comprising the following steps: constructing a collision detection model; performing collision detection on the initial pose and the target pose of the mechanical arm through a collision detection model; when the initial pose and the target pose are not collided, invoking a joint limit expression to sample in a joint space to obtain a sampling point; performing collision detection on the sampling points through a collision detection model, and reserving sampling points without collision; and planning to obtain a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points, optimizing and smoothing the mechanical arm motion track, and performing time parameterization on the mechanical arm motion track. The method can effectively avoid collision, can adapt to the kinematic constraint condition of joint limit coupling change caused by complex arm support configuration, ensures correct and effective solving result, can enable the mechanical arm to move continuously and smoothly, and reduces energy consumption.
Description
Technical Field
The application relates to the technical field of mechanical engineering, in particular to a planning method of a mechanical arm movement track in a tunnel environment; the invention also relates to a planning device, equipment and a computer readable storage medium for the movement track of the mechanical arm in the tunnel environment.
Background
Robotics have become increasingly widely used in tunnel construction. Reasonably planning the motion trail of the mechanical arm can certainly improve the construction efficiency and improve the stability of the mechanical arm. Aiming at planning of the motion trail of the mechanical arm, the existing planning scheme is divided according to planning space, and mainly comprises two types of joint space planning and Cartesian space planning. However, the solution of cartesian space planning can reduce the robot end trajectory error, but the calculation amount is very large. In the joint space planning scheme, interpolation is mostly carried out between joint space starting points, and a track curve is obtained through fitting. However, the external environment of the existing scheme is simple, and the technical defects of collision detection, low applicability, high energy consumption and the like are overcome.
In view of this, how to solve the above technical defects has become a technical problem to be solved by those skilled in the art.
Disclosure of Invention
The purpose of the application is to provide a planning method for the movement track of the mechanical arm in the tunnel environment, which can effectively avoid the occurrence of collision conditions in the tunnel environment, can adapt to the kinematic constraint condition of the joint limit coupling change caused by the complex arm support configuration, ensures the correct and effective solving result, can ensure the continuous and smooth movement of the mechanical arm, and reduces the energy consumption. Another object of the present application is to provide a planning apparatus, a device, and a computer readable storage medium for a motion track of a mechanical arm in a tunnel environment, which all have the above technical effects.
In order to solve the technical problem, the application provides a planning method for a movement track of a mechanical arm in a tunnel environment, which comprises the following steps:
constructing a collision detection model;
performing collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model;
when the initial pose and the target pose are not collided, a joint limit expression is called to sample in a joint space, so that a plurality of sampling points are obtained; the joint limit expression is a polynomial;
performing collision detection on the sampling points through the collision detection model, and reserving the sampling points without collision;
planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points;
optimizing and smoothing the motion trail of the mechanical arm to obtain an optimal motion trail of the mechanical arm;
and performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
Optionally, the constructing the collision detection model includes:
acquiring collision detection information; the collision detection information comprises a bounding box model for replacing a mechanical arm connecting rod, a transformation matrix from a trolley to the ground, a transformation matrix from a face to the trolley, a tunnel design contour or a tunnel point cloud contour and arm support model parameters;
and constructing the collision detection model according to the collision detection information.
Optionally, the planning to obtain the motion trail of the collision-free mechanical arm according to the initial pose, the target pose and the collision-free sampling point includes:
searching the target pose in a space formed by the sampling points without collision from the initial pose by using a rapid searching random tree algorithm to obtain a plurality of local paths;
performing collision detection on the local path through the collision detection model, and reserving the local path without collision;
and planning to obtain the motion trail of the mechanical arm without collision according to the local path without collision.
Optionally, performing the track optimization on the motion track of the mechanical arm includes:
sampling to obtain collision-free substitute points of the sampling points in the motion trail of the mechanical arm;
judging whether the collision-free substitution point meets a preset substitution condition or not;
if the collision-free substitute point meets the substitute condition, using the collision-free substitute point to substitute the corresponding sampling point;
and if the collision-free substitution point does not meet the substitution condition, keeping the sampling point unchanged.
Optionally, performing smoothing on the motion track of the mechanical arm to obtain a smooth motion track of the mechanical arm includes:
and taking midpoints of two adjacent points in the mechanical arm movement track, and sequentially connecting the initial pose, each midpoint and the target pose to obtain a smooth mechanical arm movement track.
For solving the technical problem, the application also provides a planning device for the movement track of the mechanical arm in the tunnel environment, comprising:
the detection model construction module is used for constructing a collision detection model;
the first detection module is used for carrying out collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model;
the sampling module is used for calling a joint limit expression to sample in a joint space when the initial pose and the target pose are not collided, so as to obtain a plurality of sampling points; the joint limit expression is a polynomial;
the second detection module is used for carrying out collision detection on the sampling points through the collision detection model and reserving the sampling points without collision;
the track planning module is used for planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points;
the track processing module is used for optimizing and smoothing the motion track of the mechanical arm to obtain an optimal motion track of the mechanical arm;
and the time parameterization module is used for performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
Optionally, the track planning module includes:
the local path construction unit is used for searching the target pose in the space formed by the sampling points without collision from the initial pose by utilizing a rapid search random tree algorithm to obtain a plurality of local paths;
a collision detection unit configured to perform collision detection on the local path by the collision detection model, and to reserve the local path without collision;
and the motion track planning unit is used for planning and obtaining the motion track of the mechanical arm without collision according to the local path without collision.
Optionally, the track processing module includes:
the midpoint selecting unit is used for selecting midpoints of two adjacent points in the motion trail of the mechanical arm;
and the connecting unit is used for sequentially connecting the initial pose, each midpoint and the target pose to obtain a smooth mechanical arm movement track.
For solving the technical problem, the application also provides planning equipment for the movement track of the mechanical arm in the tunnel environment, comprising:
a memory for storing a computer program;
and the processor is used for realizing the steps of the planning method for the movement track of the mechanical arm in the tunnel environment when executing the computer program.
In order to solve the above technical problem, the present application further provides a computer readable storage medium, where a computer program is stored on the computer readable storage medium, where the computer program when executed by a processor implements the steps of the method for planning a movement track of a mechanical arm in a tunnel environment as described in any one of the above.
The method for planning the movement track of the mechanical arm in the tunnel environment comprises the following steps: constructing a collision detection model; performing collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model; when the initial pose and the target pose are not collided, a joint limit expression is called to sample in a joint space, so that a plurality of sampling points are obtained; the joint limit expression is a polynomial; performing collision detection on the sampling points through the collision detection model, and reserving the sampling points without collision; planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points; optimizing and smoothing the motion trail of the mechanical arm to obtain an optimal motion trail of the mechanical arm; and performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
Therefore, according to the planning method for the movement track of the mechanical arm in the tunnel environment, collision detection is carried out in the planning process of the movement track of the mechanical arm, the movement track of the mechanical arm obtained through planning is guaranteed not to collide, and collision between the arm support and the tunnel design contour, oil pipe and the like can be effectively avoided in the tunnel environment. When sampling is performed in the joint space, the joint limit expression which is a polynomial is adopted for sampling, so that the method can be well adapted to the kinematic constraint condition of joint limit coupling change caused by complex arm support configuration, and the accuracy of a solving result is ensured. In addition, the method and the device further optimize and smooth the movement track of the mechanical arm on the basis of obtaining the movement track of the mechanical arm through preliminary planning, so that the movement of the mechanical arm is continuous and smooth, and the energy consumption is reduced.
The planning device, the equipment and the computer-readable storage medium for the movement track of the mechanical arm in the tunnel environment have the technical effects.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following description will briefly explain the drawings needed in the prior art and embodiments, and it is obvious that the drawings in the following description are only some embodiments of the present application, and that other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a flow chart of a method for planning a motion track of a mechanical arm in a tunnel environment according to an embodiment of the present application;
FIG. 2 is a schematic diagram of a track curve, a velocity curve and an acceleration curve according to an embodiment of the present disclosure;
fig. 3 is a schematic diagram of a planning apparatus for a motion track of a mechanical arm in a tunnel environment according to an embodiment of the present application;
fig. 4 is a schematic diagram of a planning apparatus for a movement track of a mechanical arm in a tunnel environment according to an embodiment of the present application.
Detailed Description
The core of the application is to provide a planning method for the movement track of the mechanical arm in the tunnel environment, which can effectively avoid collision conditions in the tunnel environment, can adapt to the kinematic constraint conditions of joint limit coupling changes caused by complex arm support configurations, ensures correct and effective solving results, can enable the movement of the mechanical arm to be continuous and smooth, and reduces energy consumption. Another core of the present application is to provide a planning device, a device and a computer readable storage medium for a motion track of a mechanical arm in a tunnel environment, which all have the above technical effects.
For the purposes of making the objects, technical solutions and advantages of the embodiments of the present application more clear, the technical solutions of the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is apparent that the described embodiments are some embodiments of the present application, but not all embodiments. All other embodiments, which can be made by one of ordinary skill in the art without undue burden from the present disclosure, are within the scope of the present disclosure.
Referring to fig. 1, fig. 1 is a flow chart of a method for planning a motion track of a mechanical arm in a tunnel environment according to an embodiment of the present application, and referring to fig. 1, the method mainly includes:
s101: constructing a collision detection model;
the collision detection model is used for performing collision detection according to the change condition of the arm support joint value.
In some embodiments, the collision detection model is constructed by: acquiring collision detection information; the collision detection information comprises a bounding box model for replacing a mechanical arm connecting rod, a transformation matrix from a trolley to the ground, a transformation matrix from a face to the trolley, a tunnel design contour or a tunnel point cloud contour and arm support model parameters; and constructing the collision detection model according to the collision detection information.
The bounding box model is characterized in that in the collision detection of the mechanical arm, a direction bounding box detection algorithm is adopted, the direction bounding box is represented through a transformation matrix, a center point and 3 1/2 side lengths, and a plurality of direction bounding boxes with different sizes are used for approximately replacing the mechanical arm according to the actual condition of the mechanical arm.
According to the collision detection information, the process of constructing the collision detection model mainly comprises the following steps: and obtaining the inclination angle of the car body according to the transformation matrix from the trolley to the ground, and updating the information of the catenary. And further combining a transformation matrix from the tunnel face to the trolley, a tunnel design contour or a tunnel point cloud contour and arm support model parameters, and constructing to obtain a collision detection model.
The content of the collision detection includes: and detecting collision between the mechanical arms and the tunnel environment, and between the mechanical arms and the hydraulic pipeline and between the mechanical arms and the tunnel environment. The mechanical arm connecting rod is replaced by the direction bounding box, the hydraulic pipeline is replaced by the discrete catenary, and during collision detection, the intersection detection is carried out on the direction bounding box and the direction bounding box, and the intersection detection is carried out on the direction bounding box and the line segment. The tunnel design contour takes a plurality of discrete points, converts the discrete points into a plane, and carries out intersection detection on the directional bounding box and the plane.
It can be seen that the orientation bounding boxes on the same link are not detected with each other, and the orientation bounding boxes of adjacent links are not detected with each other. If the tunnel point cloud contour exists, intersection detection can be performed on the directional bounding box and the tunnel point cloud contour, so that the intersection detection between the directional bounding box and the tunnel design contour is replaced.
S102: performing collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model;
inputting the initial pose and the target pose of the mechanical arm, and performing collision detection on the initial pose and the target pose through a collision detection model. For the target pose, the joint sensor value of the target pose needs to be obtained by solving through an inverse kinematics method, for example, the joint sensor value of the target pose is obtained by solving the upper limit 100 times through an inverse kinematics method, and then collision detection is performed.
S103: when the initial pose and the target pose are not collided, a joint limit expression is called to sample in a joint space, so that a plurality of sampling points are obtained; the joint limit expression is a polynomial;
if no collision between the initial pose and the target pose is detected, further sampling in joint space and subsequent steps are performed. If collision exists between any one of the initial pose and the target pose is detected, the mechanical arm motion track planning flow is ended, sampling and subsequent steps in the joint space are not executed any more, and collision information can be returned. In addition, if any one of the initial pose or the target pose is not solved, corresponding error information can be returned.
The joint limits in the joint space are not fixed, and the joint limits have coupling phenomenon, so that the embodiment utilizes a polynomial to construct an expression of the joint limits, then uses Gaussian distribution sampling, calls the joint limit expression to sample in the joint space, and outputs all sampling points obtained by sampling.
S104: performing collision detection on the sampling points through the collision detection model, and reserving the sampling points without collision;
s105: planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points;
on the basis of sampling the obtained sampling points, firstly, collision detection is carried out on the sampling points through a collision detection model, the sampling points with collision are deleted, and the sampling points without collision are reserved. And then, planning to obtain a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling point.
In some embodiments, the method for planning a motion track of the collision-free mechanical arm according to the starting pose, the target pose and the collision-free sampling point includes: searching the target pose in a space formed by the sampling points without collision from the initial pose by using a rapid searching random tree algorithm to obtain a plurality of local paths; performing collision detection on the local path through the collision detection model, and reserving the local path without collision; and planning to obtain the motion trail of the mechanical arm without collision according to the local path without collision.
S106: optimizing and smoothing the motion trail of the mechanical arm to obtain an optimal motion trail of the mechanical arm;
the optimization processing of the motion trail of the mechanical arm aims at shortening the motion trail of the mechanical arm. And carrying out smoothing treatment on the motion trail of the mechanical arm to obtain a smooth motion trail.
In some embodiments, performing trajectory optimization on the motion trajectory of the robotic arm includes:
sampling to obtain collision-free substitute points of the sampling points in the motion trail of the mechanical arm;
judging whether the collision-free substitution point meets a preset substitution condition or not;
if the collision-free substitute point meets the substitute condition, using the collision-free substitute point to substitute the corresponding sampling point;
and if the collision-free substitution point does not meet the substitution condition, keeping the sampling point unchanged.
The method for judging whether the collision-free substitution point meets the preset substitution condition may be as follows: for each collision-free substitute point near the sampling point, respectively according to the formula
Calculating to obtain epsilon j 。ε j Collision-free substitution point for reflecting sampling point xi>
The distance from the sample point xi-1 and the sample point xi+1. According to the formula
Epsilon is calculated. Epsilon is used to reflect the distance size of the sample point xi from sample point xi-1 and sample point xi + 1. Comparison of epsilon j And epsilon. If epsilon j < ε, and ε j Minimum, no collision substitution point->
The preset substitution condition is satisfied. If epsilon j And if the collision-free substitution points are not equal to epsilon, all the collision-free substitution points do not meet the preset substitution conditions.
In some embodiments, smoothing the motion trajectory of the mechanical arm includes: and taking midpoints of two adjacent points in the mechanical arm movement track, and sequentially connecting the initial pose, each midpoint and the target pose to obtain a smooth mechanical arm movement track.
In order to make the motion track of the mechanical arm smoother, a plurality of smoothing treatments, for example, 5 smoothing treatments, are needed.
After one smoothing process, the points on the motion track of the mechanical arm comprise:
x 1 representing the initial pose, x n Representing the pose of the target, x 2 、x 3 ……x n-1 Representing the sampling points.
After two smoothing processes, the points on the motion track of the mechanical arm comprise:
and the like, the motion trail of the mechanical arm after 5 times or more of smoothing treatment can be obtained.
S107: and performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
And after obtaining the optimal mechanical arm movement track, performing time parameterization processing on the optimal mechanical arm movement track according to the speed limit information and the acceleration limit information of the mechanical arm to obtain time points, speed and acceleration information.
Specifically, according to the speed limit information of each joint of the mechanical arm, initializing the movement time between every two track points in the movement track of the mechanical arm, and fitting by using a cubic spline interpolation function to obtain a continuous micro speed curve and a continuous acceleration curve. According to the acceleration limit information of the mechanical arm joint, the movement time between track points is adjusted, and a cubic spline interpolation function is called to perform curve fitting until the speed and the acceleration of the mechanical arm movement track are all within the limit range of the speed and the acceleration of the joint, so that the time parameterization of the mechanical arm movement track is completed, and the obtained speed curve, acceleration curve and track curve can be shown by referring to FIG. 2.
In summary, according to the planning method for the movement track of the mechanical arm in the tunnel environment, collision detection is performed simultaneously in the planning process of the movement track of the mechanical arm, so that the movement track of the mechanical arm obtained by planning is guaranteed not to collide, and collision between the arm support and the tunnel design contour, oil pipe and the like can be effectively avoided in the tunnel environment. When sampling is performed in the joint space, the joint limit expression which is a polynomial is adopted for sampling, so that the method can be well adapted to the kinematic constraint condition of joint limit coupling change caused by complex arm support configuration, and the accuracy of a solving result is ensured. In addition, on the basis of obtaining the motion trail of the mechanical arm through preliminary planning, the motion trail of the mechanical arm is further subjected to smoothing treatment, so that the motion of the mechanical arm is continuous and smooth, and the energy consumption is reduced.
The application also provides a planning device for the movement track of the mechanical arm in the tunnel environment, and the device described below can be referred to correspondingly with the method described above. Referring to fig. 3, fig. 3 is a schematic diagram of a planning apparatus for a movement track of a mechanical arm in a tunnel environment according to an embodiment of the present application, and with reference to fig. 3, the apparatus includes:
a detection model construction module 10 for constructing a collision detection model;
the first detection module 20 is configured to perform collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model;
the sampling module 30 is configured to invoke a joint limit expression to sample in a joint space when the initial pose and the target pose are not collided, so as to obtain a plurality of sampling points; the joint limit expression is a polynomial;
a second detection module 40, configured to perform collision detection on the sampling points through the collision detection model, and reserve the sampling points without collision;
the track planning module 50 is configured to plan to obtain a collision-free mechanical arm motion track according to the initial pose, the target pose, and the collision-free sampling point;
the track processing module 60 is configured to optimize and smooth the motion track of the mechanical arm to obtain an optimal motion track of the mechanical arm;
the time parameterization module 70 is configured to perform time parameterization on the optimal mechanical arm motion trajectory, so as to obtain time point, speed and acceleration information of the optimal mechanical arm motion trajectory.
On the basis of the above embodiment, as a specific implementation manner, the detection model building module 10 includes:
an information acquisition unit configured to acquire collision detection information; the collision detection information comprises a bounding box model for replacing a mechanical arm connecting rod, a transformation matrix from a trolley to the ground, a transformation matrix from a face to the trolley, a tunnel design contour or a tunnel point cloud contour and arm support model parameters;
and a model construction unit for constructing the collision detection model according to the collision detection information.
Based on the above embodiment, as a specific implementation manner, the trajectory planning module 50 includes:
the local path construction unit is used for searching the target pose in the space formed by the sampling points without collision from the initial pose by utilizing a rapid search random tree algorithm to obtain a plurality of local paths;
a collision detection unit configured to perform collision detection on the local path by the collision detection model, and to reserve the local path without collision;
and the motion track planning unit is used for planning and obtaining the motion track of the mechanical arm without collision according to the local path without collision.
Based on the above embodiment, as a specific implementation manner, the track processing module 60 includes:
the local path construction unit is used for searching the target pose in the space formed by the sampling points without collision from the initial pose by utilizing a rapid search random tree algorithm to obtain a plurality of local paths;
a collision detection unit configured to perform collision detection on the local path by the collision detection model, and to reserve the local path without collision;
and the motion track planning unit is used for planning and obtaining the motion track of the mechanical arm without collision according to the local path without collision.
Based on the above embodiment, as a specific implementation manner, the track processing module 60 includes:
the midpoint selecting unit is used for selecting midpoints of two adjacent points in the motion trail of the mechanical arm;
and the connecting unit is used for sequentially connecting the initial pose, each midpoint and the target pose to obtain a smooth mechanical arm movement track.
According to the planning device for the movement track of the mechanical arm in the tunnel environment, collision detection is carried out simultaneously in the planning process of the movement track of the mechanical arm, the movement track of the mechanical arm obtained through planning is guaranteed not to collide, and collision between the arm support and the tunnel design contour, oil pipe and the like can be effectively avoided in the tunnel environment. When sampling is performed in the joint space, the joint limit expression which is a polynomial is adopted for sampling, so that the method can be well adapted to the kinematic constraint condition of joint limit coupling change caused by complex arm support configuration, and the accuracy of a solving result is ensured. In addition, on the basis of obtaining the motion trail of the mechanical arm through preliminary planning, the motion trail of the mechanical arm is further subjected to smoothing treatment, so that the motion of the mechanical arm is continuous and smooth, and the energy consumption is reduced.
The application also provides planning equipment for the movement track of the mechanical arm in the tunnel environment, and the equipment comprises a memory 1 and a processor 2, as shown in reference to fig. 4.
A memory 1 for storing a computer program;
a processor 2 for executing a computer program to perform the steps of:
constructing a collision detection model; performing collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model; when the initial pose and the target pose are not collided, a joint limit expression is called to sample in a joint space, so that a plurality of sampling points are obtained; the joint limit expression is a polynomial; performing collision detection on the sampling points through the collision detection model, and reserving the sampling points without collision; planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points; optimizing and smoothing the motion trail of the mechanical arm to obtain an optimal motion trail of the mechanical arm; and performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
For the description of the apparatus provided in the present application, reference is made to the above method embodiments, and the description is omitted herein.
The present application also provides a computer readable storage medium having a computer program stored thereon, which when executed by a processor, performs the steps of:
constructing a collision detection model; performing collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model; when the initial pose and the target pose are not collided, a joint limit expression is called to sample in a joint space, so that a plurality of sampling points are obtained; the joint limit expression is a polynomial; performing collision detection on the sampling points through the collision detection model, and reserving the sampling points without collision; planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points; optimizing and smoothing the motion trail of the mechanical arm to obtain an optimal motion trail of the mechanical arm; and performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
The computer readable storage medium may include: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
For the description of the computer-readable storage medium provided in the present application, reference is made to the above method embodiments, and the description is omitted herein.
In the description, each embodiment is described in a progressive manner, and each embodiment is mainly described by the differences from other embodiments, so that the same similar parts among the embodiments are mutually referred. For the apparatus, device and computer readable storage medium of the embodiment disclosure, since it corresponds to the method of the embodiment disclosure, the description is relatively simple, and the relevant points refer to the description of the method section.
Those of skill would further appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative elements and steps are described above generally in terms of functionality in order to clearly illustrate the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. The software modules may be disposed in Random Access Memory (RAM), memory, read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
The method, the device, the equipment and the computer readable storage medium for planning the movement track of the mechanical arm in the tunnel environment provided by the application are described in detail. Specific examples are set forth herein to illustrate the principles and embodiments of the present application, and the description of the examples above is only intended to assist in understanding the methods of the present application and their core ideas. It should be noted that it would be obvious to those skilled in the art that various improvements and modifications can be made to the present application without departing from the principles of the present application, and such improvements and modifications fall within the scope of the claims of the present application.
Claims (10)
1. The method for planning the movement track of the mechanical arm in the tunnel environment is characterized by comprising the following steps of:
constructing a collision detection model;
performing collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model;
when the initial pose and the target pose are not collided, a joint limit expression is called to sample in a joint space, so that a plurality of sampling points are obtained; the joint limit expression is a polynomial;
performing collision detection on the sampling points through the collision detection model, and reserving the sampling points without collision;
planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points;
optimizing and smoothing the motion trail of the mechanical arm to obtain an optimal motion trail of the mechanical arm;
and performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
2. The planning method of claim 1, wherein the constructing a collision detection model comprises:
acquiring collision detection information; the collision detection information comprises a bounding box model for replacing a mechanical arm connecting rod, a transformation matrix from a trolley to the ground, a transformation matrix from a face to the trolley, a tunnel design contour or a tunnel point cloud contour and arm support model parameters;
and constructing the collision detection model according to the collision detection information.
3. The planning method according to claim 1, wherein the planning to obtain the collision-free mechanical arm motion track according to the starting pose, the target pose and the collision-free sampling point includes:
searching the target pose in a space formed by the sampling points without collision from the initial pose by using a rapid searching random tree algorithm to obtain a plurality of local paths;
performing collision detection on the local path through the collision detection model, and reserving the local path without collision;
and planning to obtain the motion trail of the mechanical arm without collision according to the local path without collision.
4. The planning method of claim 1, wherein performing trajectory optimization on the robot motion trajectory comprises:
sampling to obtain collision-free substitute points of the sampling points in the motion trail of the mechanical arm;
judging whether the collision-free substitution point meets a preset substitution condition or not;
if the collision-free substitute point meets the substitute condition, using the collision-free substitute point to substitute the corresponding sampling point;
and if the collision-free substitution point does not meet the substitution condition, keeping the sampling point unchanged.
5. The planning method according to claim 1, wherein smoothing the motion trajectory of the mechanical arm includes:
and taking midpoints of two adjacent points in the mechanical arm movement track, and sequentially connecting the initial pose, each midpoint and the target pose to obtain a smooth mechanical arm movement track.
6. Planning device of arm motion track under tunnel environment, its characterized in that includes:
the detection model construction module is used for constructing a collision detection model;
the first detection module is used for carrying out collision detection on the initial pose and the target pose of the mechanical arm through the collision detection model;
the sampling module is used for calling a joint limit expression to sample in a joint space when the initial pose and the target pose are not collided, so as to obtain a plurality of sampling points; the joint limit expression is a polynomial;
the second detection module is used for carrying out collision detection on the sampling points through the collision detection model and reserving the sampling points without collision;
the track planning module is used for planning and obtaining a collision-free mechanical arm motion track according to the initial pose, the target pose and the collision-free sampling points;
the track processing module is used for optimizing and smoothing the motion track of the mechanical arm to obtain an optimal motion track of the mechanical arm;
and the time parameterization module is used for performing time parameterization on the optimal mechanical arm movement track to obtain the time point, the speed and the acceleration information of the optimal mechanical arm movement track.
7. The planning apparatus of claim 6, wherein the trajectory planning module comprises:
the local path construction unit is used for searching the target pose in the space formed by the sampling points without collision from the initial pose by utilizing a rapid search random tree algorithm to obtain a plurality of local paths;
a collision detection unit configured to perform collision detection on the local path by the collision detection model, and to reserve the local path without collision;
and the motion track planning unit is used for planning and obtaining the motion track of the mechanical arm without collision according to the local path without collision.
8. The planning apparatus of claim 6, wherein the trajectory processing module comprises:
the midpoint selecting unit is used for selecting midpoints of two adjacent points in the motion trail of the mechanical arm;
and the connecting unit is used for sequentially connecting the initial pose, each midpoint and the target pose to obtain a smooth mechanical arm movement track.
9. Planning equipment of arm motion track under tunnel environment, its characterized in that includes:
a memory for storing a computer program;
a processor, configured to implement the steps of the method for planning a movement track of a mechanical arm in a tunnel environment according to any one of claims 1 to 5 when executing the computer program.
10. A computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and the computer program, when executed by a processor, implements the steps of the method for planning a movement track of a mechanical arm in a tunnel environment according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210433946.1A CN114633258B (en) | 2022-04-24 | 2022-04-24 | Planning method and related device for movement track of mechanical arm in tunnel environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210433946.1A CN114633258B (en) | 2022-04-24 | 2022-04-24 | Planning method and related device for movement track of mechanical arm in tunnel environment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114633258A CN114633258A (en) | 2022-06-17 |
| CN114633258B true CN114633258B (en) | 2023-06-20 |
Family
ID=81952070
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210433946.1A Active CN114633258B (en) | 2022-04-24 | 2022-04-24 | Planning method and related device for movement track of mechanical arm in tunnel environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114633258B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114851211B (en) * | 2022-07-07 | 2022-09-23 | 国网瑞嘉(天津)智能机器人有限公司 | Method, device, server and storage medium for planning track of boom |
| CN114986524B (en) * | 2022-08-08 | 2022-10-28 | 中国科学院自动化研究所 | Mechanical arm track planning method and device based on brain-computer interface and electronic equipment |
| CN115870976A (en) * | 2022-11-16 | 2023-03-31 | 北京洛必德科技有限公司 | Sampling trajectory planning method and device for mechanical arm and electronic equipment |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101733750A (en) * | 2009-11-05 | 2010-06-16 | 浙江大学 | Dynamic response spectrum-based method for planning track of robot with clearance at a joint |
| CN104526695A (en) * | 2014-12-01 | 2015-04-22 | 北京邮电大学 | Space manipulator track planning method for minimizing base seat collision disturbance |
| CN109048892A (en) * | 2018-07-26 | 2018-12-21 | 西北工业大学 | A kind of mechanical arm tail end barrier-avoiding method based on Q study |
| CN111070206A (en) * | 2019-12-13 | 2020-04-28 | 同济大学 | Station layout method for reducing robot motion energy consumption |
| CN113954077A (en) * | 2021-11-15 | 2022-01-21 | 天津大学 | Underwater swimming mechanical arm trajectory tracking control method and device with energy optimization function |
Family Cites Families (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101667031B1 (en) * | 2009-11-02 | 2016-10-17 | 삼성전자 주식회사 | Path planning apparatus of robot and method thereof |
| WO2012143044A1 (en) * | 2011-04-19 | 2012-10-26 | Abb Research Ltd. | An industrial robot having a kinematically redundant arm and a method for controlling the robot |
| JP6598090B2 (en) * | 2015-01-26 | 2019-10-30 | デューク・ユニバーシティ | Storage medium, method for planning robot operation, hardware for planning robot operation, and robot |
| CN108582073B (en) * | 2018-05-02 | 2020-09-15 | 北京邮电大学 | Mechanical arm rapid obstacle avoidance method based on improved random road marking map method |
| CN109968358B (en) * | 2019-03-28 | 2021-04-09 | 北京工业大学 | Redundant robot full-joint obstacle avoidance track optimization method considering motion stability |
| CN109986564A (en) * | 2019-05-20 | 2019-07-09 | 上海应用技术大学 | Industrial machinery arm paths planning method |
| CN110340890A (en) * | 2019-06-27 | 2019-10-18 | 北京控制工程研究所 | A kind of space manipulator overall situation is without touching Trajectory Planning System |
| CN111152224B (en) * | 2020-01-10 | 2022-05-10 | 温州大学 | Dual-optimization robot motion trajectory optimization method |
| CN115515523A (en) * | 2020-03-30 | 2022-12-23 | 奥瑞斯健康公司 | Workspace optimization for robotic surgery |
| CN111515953B (en) * | 2020-04-29 | 2021-09-03 | 北京阿丘机器人科技有限公司 | Path planning method and device and electronic equipment |
| CN112223291B (en) * | 2020-10-21 | 2022-02-11 | 哈工大机器人（合肥）国际创新研究院 | Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint |
| CN113103236B (en) * | 2021-04-22 | 2022-06-10 | 山东大学 | Rapid and gradual optimal mechanical arm obstacle avoidance path planning method |
| CN114290332B (en) * | 2021-12-28 | 2024-05-07 | 哈尔滨工业大学 | Series mechanical arm path planning method and system applied to GIS pipeline detection |
-
2022
- 2022-04-24 CN CN202210433946.1A patent/CN114633258B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101733750A (en) * | 2009-11-05 | 2010-06-16 | 浙江大学 | Dynamic response spectrum-based method for planning track of robot with clearance at a joint |
| CN104526695A (en) * | 2014-12-01 | 2015-04-22 | 北京邮电大学 | Space manipulator track planning method for minimizing base seat collision disturbance |
| CN109048892A (en) * | 2018-07-26 | 2018-12-21 | 西北工业大学 | A kind of mechanical arm tail end barrier-avoiding method based on Q study |
| CN111070206A (en) * | 2019-12-13 | 2020-04-28 | 同济大学 | Station layout method for reducing robot motion energy consumption |
| CN113954077A (en) * | 2021-11-15 | 2022-01-21 | 天津大学 | Underwater swimming mechanical arm trajectory tracking control method and device with energy optimization function |
Non-Patent Citations (2)
| Title |
|---|
| 基于二次型规划的平面冗余机械臂的自运动;张雨浓;谭志国;杨智;蔡炳煌;;机器人(06);第566-571页 * |
| 机器人关节空间障碍的特性分析;王伟,储林波;机械科学与技术(03);第419-423页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114633258A (en) | 2022-06-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114633258B (en) | Planning method and related device for movement track of mechanical arm in tunnel environment | |
| CN112223291B (en) | Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint | |
| KR101056600B1 (en) | Industrial Product Design Method Using Closoid Curve and Numerical Control Method and Apparatus Using Industrial Product Designed by Two Design Method | |
| CN113352319B (en) | Redundant mechanical arm obstacle avoidance trajectory planning method based on improved fast expansion random tree | |
| CN111251306B (en) | Mechanical arm path planning method with chassis error | |
| CN108638055B (en) | Autonomous obstacle avoidance planning method for seven-degree-of-freedom space manipulator | |
| CN113172631B (en) | Mechanical arm autonomous obstacle avoidance method based on improved RRT algorithm | |
| CN110285802A (en) | Quick Extended random tree path smoothing method | |
| CN112549016A (en) | Mechanical arm motion planning method | |
| CN112356033B (en) | Mechanical arm path planning method integrating low-difference sequence and RRT algorithm | |
| CN113799120B (en) | Path planning method and device for redundant degree of freedom mechanical arm and engineering machine | |
| CN110561419A (en) | arm-shaped line constraint flexible robot track planning method and device | |
| CN113771035B (en) | Redundant multi-degree-of-freedom mechanical arm obstacle avoidance path optimization method based on RRT algorithm | |
| CN115958590A (en) | RRT-based mechanical arm deep frame obstacle avoidance motion planning method and device | |
| CN114260625B (en) | Welding method, welding equipment and storage medium for circular tube intersecting line | |
| CN117162098B (en) | Autonomous planning system and method for robot gesture in narrow space | |
| CN111958602B (en) | Real-time inverse solution method for wrist offset type 6-axis robot | |
| CN114488941A (en) | Trace fairing method and medium for micro line segments and machine tool numerical control equipment | |
| Riboli et al. | Collision-free and smooth motion planning of dual-arm Cartesian robot based on B-spline representation | |
| CN116690557A (en) | Method and device for controlling humanoid three-dimensional scanning motion based on point cloud | |
| CN115981311A (en) | Path planning method and system for complex pipeline maintenance robot | |
| CN115922716A (en) | Bidirectional RRT-connect algorithm fused with process knowledge realizes rapid path planning of industrial robot | |
| CN112894817B (en) | Mechanical arm motion planning method in task space | |
| CN117182932B (en) | Method and device for planning obstacle avoidance action of mechanical arm and computer equipment | |
| CN113119123B (en) | Motion control method for office swivel chair production |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |