CN112549028A - Double-arm robot track planning method based on dynamic motion primitives and artificial potential field - Google Patents
Double-arm robot track planning method based on dynamic motion primitives and artificial potential field Download PDFInfo
- Publication number
- CN112549028A CN112549028A CN202011399548.XA CN202011399548A CN112549028A CN 112549028 A CN112549028 A CN 112549028A CN 202011399548 A CN202011399548 A CN 202011399548A CN 112549028 A CN112549028 A CN 112549028A
- Authority
- CN
- China
- Prior art keywords
- arm robot
- planning
- path
- dynamic motion
- term
- 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
- 230000033001 locomotion Effects 0.000 title claims abstract description 133
- 238000000034 method Methods 0.000 title claims abstract description 88
- 230000006870 function Effects 0.000 claims description 51
- 230000001133 acceleration Effects 0.000 claims description 38
- 238000006073 displacement reaction Methods 0.000 claims description 9
- 238000013016 damping Methods 0.000 claims description 6
- 230000009471 action Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 description 14
- 238000010586 diagram Methods 0.000 description 3
- 239000012636 effector Substances 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 201000004569 Blindness Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- 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
-
- 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/1679—Programme controls characterised by the tasks executed
- B25J9/1682—Dual arm manipulator; Coordination of several manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention belongs to the field of trajectory planning, and particularly relates to a method, a system and a device for planning a trajectory of a double-arm robot based on dynamic motion primitives and artificial potential fields, aiming at solving the problems of poor instantaneity, poor generalization and low efficiency of the conventional robot trajectory planning method. The system method comprises the steps of obtaining a target position of the double-arm robot to be subjected to path planning as input data; based on input data, the double-arm robot obtains a corresponding planning path through a pre-constructed dynamic primitive motion model and moves to a target position along the planning path. The invention improves the real-time performance, generalization and efficiency of the robot trajectory planning.
Description
Technical Field
The invention belongs to the field of robot trajectory planning, and particularly relates to a method, a system and a device for planning a trajectory of a double-arm robot based on dynamic motion primitives and artificial potential fields.
Background
The trajectory planning is the basis of the control of the mechanical arm, is one of key technologies for controlling the motion of the double-arm robot, and has important significance on the working efficiency, the motion stability and the like of the mechanical arm. Common trajectory planning algorithms include spline interpolation, fast-search random trees (RRT), artificial potential field methods, and the like. The trajectory planning method based on spline interpolation contains a time constant, so that the multi-degree-of-freedom trajectory learning is difficult; track continuity generated by a method based on statistics, such as a fast exploration random tree, is poor, path roundabout and deadlock can occur in a complex environment, and due to the searching blindness, the iteration times are increased, the calculation amount is greatly increased, the real-time performance is poor, and the efficiency is low; the artificial potential field method is a mature and efficient planning method in a trajectory planning algorithm, and a simple mathematical model of the artificial potential field method is widely used, but the artificial potential field method is easy to fall into a local minimum value. Based on the dynamic movement primitive and the artificial potential field, the invention provides a double-arm robot track planning method based on the dynamic movement primitive and the artificial potential field.
Disclosure of Invention
In order to solve the above problems in the prior art, that is, to solve the problems of poor real-time performance, poor generalization performance and low efficiency of the existing robot trajectory planning method, the first aspect of the present invention provides a dual-arm robot trajectory planning method based on dynamic motion primitives and artificial potential fields, the method comprising:
step S10, acquiring a target position of the dual-arm robot for path planning as input data;
step S20, based on the input data, the double-arm robot obtains a corresponding planning path through a pre-constructed dynamic motion primitive model and moves to the target position along the planning path;
the dynamic motion primitive model is constructed by the following method:
step A10, constructing a double-arm robot model based on a D-H parameter table of the double-arm robot and solving a working space of the double-arm robot through a Monte Carlo algorithm;
step A20, setting a starting position, a target position and a corresponding demonstration path in the working space, and acquiring track motion data of the double-arm robot when moving along the demonstration path in real time as first data; the track motion data comprises displacement, speed and acceleration;
step A30, based on the first data, obtaining a nonlinear forcing term corresponding to the demonstration path through a pre-constructed first model, as a first forcing term; the first model is a dynamic motion primitive model of discrete motion that does not contain an forcing term;
step A40, based on the first forcing term, combining the initial position and the target position of the set learning path, obtaining the optimal weight value of each basis function through a local weighting method, and constructing a nonlinear forcing term corresponding to the learning path as a second forcing term;
step A50, adding the second forcing term and the pre-constructed acceleration repulsion term into the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as a finally constructed dynamic motion primitive model; the acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.
In some preferred embodiments, the dynamic motion primitive model of discrete motion that does not contain an forcing term is:
where τ denotes a time scaling factor, αyDenotes the elastic constant, betayRepresenting the system damping term, g representing the target position, y,
Representing the position, velocity, acceleration of the primitive during motion.
In some preferred embodiments, the non-linear forcing term corresponding to the exemplary path is:
wherein f istargetActual values, y, representing non-linear forcing terms corresponding to exemplary pathsdemo、
Representing the position, velocity and acceleration of the corresponding element of the exemplary trajectory during motion, y0、g0The starting position and the target position corresponding to the exemplary track are shown.
In some preferred embodiments, the "obtaining the optimal weight value of each basis function by a local weighting method" is performed by:
S＝(ξ(1) ξ(2) ... ξ(p))T
ξ＝x(g1-y1)
wherein psiiRepresenting a center of compliance as ciVariance is hiOf a gaussian distribution, STIs shown asiDenotes, x denotes the phase variable, ωiDenotes the weight of the ith basis function, i denotes the subscript, T denotes the transpose, y1、g1Respectively representing the initial position and the target position corresponding to the learning track, and p represents the number and is a set value.
In some preferred embodiments, the non-linear forcing term corresponding to the learning path is:
wherein f represents the actual value of the nonlinear forcing term corresponding to the learning path, and N represents the number of the basis functions.
In some preferred embodiments, the acceleration rejection term is:
U(y)＝Uatt(y)+Urep(y)
wherein,
representing the actual value of the acceleration rejection term, U (y) representing the potential function at point y, Urep(y)、Uatt(y) denotes the gravitational potential, the repulsive potential at point y,
denotes the gravitational gain, d (y, g)1) Representing the current point y to the target point g1η represents the repulsive gain, d (y) represents the distance between point y and the nearest obstacle, and Q represents the obstacle distance action threshold.
In some preferred embodiments, the dynamic motion primitive model with the obstacle avoidance function is:
the invention provides a two-arm robot track planning system based on dynamic motion primitives and artificial potential fields, which comprises a position acquisition module and a path planning output module;
the position acquisition module is configured to acquire a target position of the double-arm robot for path planning as input data;
the path planning output module is configured to obtain a corresponding planning path through a pre-constructed dynamic motion primitive model by the double-arm robot based on the input data, and move to the target position along the planning path;
the dynamic motion primitive model is constructed by the following method:
step A10, constructing a double-arm robot model based on a D-H parameter table of the double-arm robot and solving a working space of the double-arm robot through a Monte Carlo algorithm;
step A20, setting a starting position, a target position and a corresponding demonstration path in the working space, and acquiring track motion data of the double-arm robot when moving along the demonstration path in real time as first data; the track motion data comprises displacement, speed and acceleration;
step A30, based on the first data, obtaining a nonlinear forcing term corresponding to the demonstration path through a pre-constructed first model, as a first forcing term; the first model is a dynamic motion primitive model of discrete motion that does not contain an forcing term;
step A40, based on the first forcing term, combining the initial position and the target position of the set learning path, obtaining the optimal weight value of each basis function through a local weighting method, and constructing a nonlinear forcing term corresponding to the learning path as a second forcing term;
step A50, adding the second forcing term and the pre-constructed acceleration repulsion term into the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as a finally constructed dynamic motion primitive model; the acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.
In a third aspect of the present invention, a storage device is provided, in which a plurality of programs are stored, the programs being adapted to be loaded and executed by a processor to implement the above-mentioned trajectory planning method for a two-arm robot based on dynamic motion primitives and artificial potential fields.
In a fourth aspect of the present invention, a processing apparatus is provided, which includes a processor, a storage device; a processor adapted to execute various programs; a storage device adapted to store a plurality of programs; the program is suitable for being loaded and executed by a processor to implement the above-mentioned dual-arm robot trajectory planning method based on dynamic motion primitives and artificial potential fields.
The invention has the beneficial effects that:
the invention improves the real-time performance, generalization and efficiency of the robot trajectory planning.
(1) The invention only needs to collect track sample data once for training, and then obtains the weight parameters of the dynamic motion primitive model, thereby realizing the autonomous track planning of the robot. On the basis, when the robot completes a new task, only the starting point, the target point and relevant parameters of the movement are modified, the sample can be maintained without retraining the sample, the characteristics of the original sample track are kept to reach a new target position, and the learning model is not specific to a specific task and has the capability of generalization and popularization.
(2) The invention applies the artificial potential field method to the dynamic motion element method, namely, a repulsive acceleration term is introduced into a second-order dynamic system with stable property, so that the generated track point is far away from the barrier, the generated track not only has the expected motion trend, but also can achieve the effect of avoiding the barrier, and the efficiency and the safety of track planning are improved.
(3) The artificial potential field method is applied to the method of dynamic motion elements, and compared with the method based on probability sampling, such as fast exploration of random trees, the method has the advantages that the learned track is more continuous; compared with an interpolation method, the method is easier to realize multi-degree-of-freedom coupling, can simulate and learn multi-degree-of-freedom tracks, and has fewer related parameters and easier parameter adjustment.
Drawings
Other features, objects and advantages of the present application will become more apparent upon reading of the following detailed description of non-limiting embodiments thereof, made with reference to the accompanying drawings.
FIG. 1 is a flow diagram illustrating a process for constructing a dynamic motion primitive model according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a framework of a two-arm robot trajectory planning system based on dynamic motion primitives and artificial potential fields in accordance with an embodiment of the present invention;
FIG. 3 is an exemplary diagram of a generated trajectory compared to a trajectory of an exemplary trajectory in accordance with one embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The present application will be described in further detail with reference to the following drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the relevant invention and not restrictive of the invention. It should be noted that, for convenience of description, only the portions related to the related invention are shown in the drawings.
It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict.
The invention discloses a double-arm robot track planning method based on dynamic motion primitives and artificial potential fields, which comprises the following steps of:
step S10, acquiring a target position of the dual-arm robot for path planning as input data;
step S20, based on the input data, the double-arm robot obtains a corresponding planning path through a pre-constructed dynamic motion primitive model and moves to the target position along the planning path;
the dynamic motion primitive model is constructed by the following method:
step A10, constructing a double-arm robot model based on a D-H parameter table of the double-arm robot and solving a working space of the double-arm robot through a Monte Carlo algorithm;
step A20, setting a starting position, a target position and a corresponding demonstration path in the working space, and acquiring track motion data of the double-arm robot when moving along the demonstration path in real time as first data; the track motion data comprises displacement, speed and acceleration;
step A30, based on the first data, obtaining a nonlinear forcing term corresponding to the demonstration path through a pre-constructed first model, as a first forcing term; the first model is a dynamic motion primitive model of discrete motion that does not contain an forcing term;
step A40, based on the first forcing term, combining the initial position and the target position of the set learning path, obtaining the optimal weight value of each basis function through a local weighting method, and constructing a nonlinear forcing term corresponding to the learning path as a second forcing term;
step A50, adding the second forcing term and the pre-constructed acceleration repulsion term into the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as a finally constructed dynamic motion primitive model; the acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.
In order to more clearly describe the trajectory planning method of the two-arm robot based on dynamic motion primitives and artificial potential fields, the following describes the steps of an embodiment of the method in detail with reference to the accompanying drawings.
In the following embodiments, the process of constructing the dynamic motion primitive model is detailed first, and then the process of acquiring the planned path by the two-arm robot trajectory planning method based on the dynamic motion primitive and the artificial potential field is detailed.
1. The dynamic motion primitive model is constructed as shown in FIG. 1
Step A10, constructing a double-arm robot model based on a D-H parameter table of the double-arm robot and solving a working space of the double-arm robot through a Monte Carlo algorithm;
in this embodiment, according to a D-H parameter table of a two-arm robot, a two-arm robot model is established by a Robotic Toolbox toolkit in MATLAB, and a monte carlo algorithm is used to solve and calculate a working space of the two-arm robot, so as to obtain a specific working range (i.e., a working space) of the robot.
Step A20, setting a starting position, a target position and a corresponding demonstration path in the working space, and acquiring track motion data of the double-arm robot when moving along the demonstration path in real time as first data; the track motion data comprises displacement, speed and acceleration;
in the present embodiment, the two-arm robot is turned on, the start position and the target position are set in the working space of the two-arm robot, and an optimum path, i.e., a demonstration path, which enables the two-arm robot to move from the start position to the target position and to avoid the obstacle is selected.
And in the motion process of the double-arm robot, acquiring track motion data in the x-y-z three-dimensional direction in a Cartesian coordinate system when the double-arm robot moves along the demonstration path in real time, wherein the track motion data comprises displacement, speed and acceleration.
Step A30, based on the first data, obtaining a nonlinear forcing term corresponding to the demonstration path through a pre-constructed first model, as a first forcing term; the first model is a dynamic motion primitive model of discrete motion that does not contain an forcing term;
in the embodiment, the parameters of the dynamic motion primitive model are obtained by training with the demonstration track samples, so that the autonomous track planning of the robot is realized.
The DMP is a nonlinear function term which introduces a series of weighted superposition of Gaussian functions in a second-order dynamic system with stable property. The movement process of the dynamic system is determined by a nonlinear function, so that the system reaches a target attractor state. The method is based on a spring-mass-damping model, and abstracts the model into a point attraction subsystem. Wherein, the dynamic motion primitive model of discrete motion without forcing term is shown as formula (1):
where τ represents a time scaling factor used to adjust the scaling speed, α, of a typical systemyDenotes the elastic constant, betayRepresenting the damping term of the system, typically betay＝αy/4, so that the system reaches a critical damping state, g represents the target position, y,
Representing the position, velocity, acceleration of the primitive during motion.
The dynamic system in equation (1) is a second-order system with only one point attractor, therefore, the system can only converge to the target position g in a specific motion form, and in order to make the system converge to the target point in the motion form we want, we introduce a non-linear forcing term f into equation (1), resulting in equations (2) (3):
where equation (3) is a canonical system of modulation time, x represents a phase variable, αxA parameter indicative of a regular system parameter,
the first derivative of x.
In the scheme, the initial parameters comprise a target position g and an elastic constant alphaySystem damping term betayThe time scaling factor τ, f is a non-linear forcing term, so that the system generates arbitrarily non-linear complex motion. Can push reversely according to the formula (2)A non-linear forcing term in the exemplary trajectory is derived, as shown in equation (4):
wherein f istargetActual values, y, representing non-linear forcing terms corresponding to exemplary pathsdemo、
Representing the position, velocity and acceleration of the corresponding element of the exemplary trajectory during motion, y0、g0The starting position and the target position corresponding to the exemplary track are shown.
Step A40, based on the first forcing term, combining the initial position and the target position of the set learning path, obtaining the optimal weight value of each basis function through a local weighting method, and constructing a nonlinear forcing term corresponding to the learning path as a second forcing term;
in the embodiment, relevant initial parameters of the dynamic motion primitive method are set according to the initial task of the robot, and the initial position and the target position of the robot learning and the calculated optimal weight value are given, so that a learning track can be generated through the dynamic motion primitive model, and the learning track has the characteristic of a demonstration track, namely, the motion trend is basically the same as the original demonstration track.
Wherein, calculating the nonlinear forcing term of the learning trajectory, as shown in the formulas (5) and (6):
wherein f represents the actual value of the non-linear forcing term corresponding to the learning path, N represents the number of basis functions, ψiRepresenting a center of compliance as ciSquare, squareThe difference is hiX represents a phase variable, ωiDenotes the weight of the ith basis function, i denotes the subscript, T denotes the transpose, y1、g1The start position and the target position corresponding to the learning trajectory are shown.
It can be seen that the basis function ψiBy weighted addition, a forcing function f is combined. Due to the basis function psiiIs non-linear and therefore the forcing function f and the whole dynamic motion primitive system is also non-linear. Equation (6) shows the basis function ψiObedience center is ciVariance is hiA gaussian distribution of (a). OmegaiAnd N is the number of the basis functions, and the more the number of the basis functions is, the smoother the generalized target track is.
The basis function weights are calculated as shown in equations (7) (8) (9) (10):
S＝(ξ(1) ξ(2) ... ξ(p))T (8)
ξ＝x(g1-y1) (9)
wherein psiiRepresenting a center of compliance as ciVariance is hiI denotes the subscript, T denotes the transpose, y denotes the inverse of1、g1Respectively representing the initial position and the target position corresponding to the learning track, and p represents the number and is a set value.
As shown in fig. 3, one exemplary trajectory (black curve) and one learning trajectory (gray curve) have characteristics of the exemplary trajectory, that is, the motion trend is substantially the same as the original exemplary trajectory.
Step A50, adding the second forcing term and the pre-constructed acceleration repulsion term into the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as a finally constructed dynamic motion primitive model; the acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.
The artificial potential field method is a common online obstacle avoidance method. In the present invention, a potential field is defined around an obstacle, while the gradient of the potential field generates a repulsive force to the robot. This method is common in the motion planning of mobile robots and is also used in robot actuators.
The invention focuses on the motion of the end effector of the mechanical arm, and compared with a joint space, the position and the potential field of an obstacle are easier to obtain in the effector space, so that a dynamic motion element model with an obstacle avoidance function can be obtained by only adding one rejection item when DMPs are used in the space for executing the DMPs. This addition allows the generated motion path to be altered by the properties of the potential function, each obstacle creating a potential field u (y) at the point of motion, i.e.
Representing the actual value of the acceleration rejection term.
In the present embodiment, the repulsive acceleration
Is the negative gradient of the artificial potential field, which depends on the relative position and velocity of the end-effector with respect to the obstacle, such that the generated motion path is altered by the properties of the potential function. The dynamic motion primitive model with an added acceleration rejection term is given by equation (11):
the position points of the obstacles in the working environment can be set to be repulsive fields U according to the actual task requirementsrep(y) setting the task target point as gravitational field Uatt(y) the motion is influenced by the total force field u (y). Addition in the formulaThe term being given by the gradient of the potential field, i.e.
The potential function u (y) at a certain point y is the sum of the gravitational potential and the repulsive potential, as shown in equation (12):
U(y)＝Uatt(y)+Urep(y) (12)
the gravitational potential function is shown as equation (13):
the repulsive potential function is shown in equation (14):
wherein,
representing the actual value of the acceleration rejection term, U (y) representing the potential function at point y, Urep(y)、Uatt(y) denotes the gravitational potential, the repulsive potential at point y,
denotes the gravitational gain, d (y, g)1) Representing the current point y to the target point g1The distance between the points is eta, the repulsion gain is shown, D (y) shows the distance between the point y and the nearest obstacle, Q shows the obstacle distance action threshold value, and the obstacle larger than the distance does not generate the repulsion influence.
In the operation process of the double-arm robot, the situation that the distance between the obstacle and the target point is close can occur, and in the situation, the situation that the target cannot be reached can occur in the artificial potential field method. Aiming at the situation, a repulsion function is improved, a relative distance item between the robot and a target point is added in an original repulsion function, as shown in the formula (14), so that the attraction and the repulsion are close to 0 simultaneously in the process that the robot moves to the target point, the resultant force borne by the robot when the robot reaches the target point is 0, and the problem that the target in the artificial potential field method cannot be reached is solved.
Therefore, the artificial potential field method is applied to the method of the dynamic motion primitive, the second-order system comprises a repulsive acceleration term, the generated track point is far away from the obstacle, and the generated track not only has an expected motion trend, but also can achieve the effect of obstacle avoidance.
Based on the constructed dynamic motion primitive model with the obstacle avoidance function, displacement, speed and acceleration information of different tracks can be calculated according to the initial positions and the target positions of the different tracks, the characteristics of the original sample track are kept to reach a new target position without retraining the sample, and the process is not specific to a specific task and has the capability of generalization and popularization.
2. Double-arm robot track planning method based on dynamic motion primitives and artificial potential field
Step S10, acquiring a target position of the dual-arm robot for path planning as input data;
in this embodiment, a target position for path planning of the dual-arm robot is obtained.
And step S20, based on the input data, the double-arm robot obtains a corresponding planning path through a pre-constructed dynamic motion primitive model, and moves to the target position along the planning path.
In this embodiment, the two-arm robot obtains a corresponding planned path through the dynamic motion primitive model with the obstacle avoidance function, and moves to the target position along the planned path.
A two-arm robot trajectory planning system based on dynamic motion primitives and artificial potential fields according to a second embodiment of the present invention, as shown in fig. 2, includes: a position acquisition module 100 and a path planning output module 200;
the position acquisition module 100 is configured to acquire a target position of the dual-arm robot for path planning as input data;
the path planning output module 200 is configured to obtain a corresponding planned path through a pre-constructed dynamic motion primitive model by the two-arm robot based on the input data, and move to the target position along the planned path;
the dynamic motion primitive model is constructed by the following method:
step A10, constructing a double-arm robot model based on a D-H parameter table of the double-arm robot and solving a working space of the double-arm robot through a Monte Carlo algorithm;
step A20, setting a starting position, a target position and a corresponding demonstration path in the working space, and acquiring track motion data of the double-arm robot when moving along the demonstration path in real time as first data; the track motion data comprises displacement, speed and acceleration;
step A30, based on the first data, obtaining a nonlinear forcing term corresponding to the demonstration path through a pre-constructed first model, as a first forcing term; the first model is a dynamic motion primitive model of discrete motion that does not contain an forcing term;
step A40, based on the first forcing term, combining the initial position and the target position of the set learning path, obtaining the optimal weight value of each basis function through a local weighting method, and constructing a nonlinear forcing term corresponding to the learning path as a second forcing term;
step A50, adding the second forcing term and the pre-constructed acceleration repulsion term into the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as a finally constructed dynamic motion primitive model; the acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.
It can be clearly understood by those skilled in the art that, for convenience and brevity of description, the specific working process and related description of the system described above may refer to the corresponding process in the foregoing method embodiment, and details are not described herein again.
It should be noted that the two-arm robot trajectory planning system based on dynamic motion primitives and artificial potential fields provided in the above embodiment is only illustrated by the division of the above functional modules, and in practical applications, the above function allocation may be completed by different functional modules according to needs, that is, the modules or steps in the embodiments of the present invention are further decomposed or combined, for example, the modules in the above embodiment may be combined into one module, or may be further split into multiple sub-modules, so as to complete all or part of the above described functions. The names of the modules and steps involved in the embodiments of the present invention are only for distinguishing the modules or steps, and are not to be construed as unduly limiting the present invention.
A storage device according to a third embodiment of the present invention stores therein a plurality of programs adapted to be loaded by a processor and to implement the above-described method for planning trajectories of a two-armed robot based on dynamic motion primitives and artificial potential fields.
A processing apparatus according to a fourth embodiment of the present invention includes a processor, a storage device; a processor adapted to execute various programs; a storage device adapted to store a plurality of programs; the program is adapted to be loaded and executed by a processor to implement the above-described method of dual-arm robot trajectory planning based on dynamic motion primitives and artificial potential fields.
It can be clearly understood by those skilled in the art that, for convenience and brevity, the specific working processes and related descriptions of the storage device and the processing device described above may refer to the corresponding processes in the foregoing method examples, and are not described herein again.
Those of skill in the art would appreciate that the various illustrative modules, method steps, and modules described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that programs corresponding to the software modules, method steps may be located 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. To clearly illustrate this interchangeability of electronic hardware and software, various illustrative components and steps have been described above generally in terms of their functionality. Whether such functionality is implemented as electronic 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 invention.
The terms "first," "second," and the like are used for distinguishing between similar elements and not necessarily for describing or implying a particular order or sequence.
The terms "comprises," "comprising," or any other similar term are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus.
So far, the technical solutions of the present invention have been described in connection with the preferred embodiments shown in the drawings, but it is easily understood by those skilled in the art that the scope of the present invention is obviously not limited to these specific embodiments. Equivalent changes or substitutions of related technical features can be made by those skilled in the art without departing from the principle of the invention, and the technical scheme after the changes or substitutions can fall into the protection scope of the invention.
Claims (10)
1. A double-arm robot track planning method based on dynamic motion primitives and artificial potential fields is characterized by comprising the following steps:
step S10, acquiring a target position of the dual-arm robot for path planning as input data;
step S20, based on the input data, the double-arm robot obtains a corresponding planning path through a pre-constructed dynamic motion primitive model and moves to the target position along the planning path;
the dynamic motion primitive model is constructed by the following method:
step A10, constructing a double-arm robot model based on a D-H parameter table of the double-arm robot and solving a working space of the double-arm robot through a Monte Carlo algorithm;
step A20, setting a starting position, a target position and a corresponding demonstration path in the working space, and acquiring track motion data of the double-arm robot when moving along the demonstration path in real time as first data; the track motion data comprises displacement, speed and acceleration;
step A30, based on the first data, obtaining a nonlinear forcing term corresponding to the demonstration path through a pre-constructed first model, as a first forcing term; the first model is a dynamic motion primitive model of discrete motion that does not contain an forcing term;
step A40, based on the first forcing term, combining the initial position and the target position of the set learning path, obtaining the optimal weight value of each basis function through a local weighting method, and constructing a nonlinear forcing term corresponding to the learning path as a second forcing term;
step A50, adding the second forcing term and the pre-constructed acceleration repulsion term into the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as a finally constructed dynamic motion primitive model; the acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.
2. The method for planning trajectories of two-arm robots based on dynamic motion primitives and artificial potential fields as claimed in claim 1, wherein the model of the dynamic motion primitives of the discrete motions without forcing term is:
where τ denotes a time scaling factor, αyDenotes the elastic constant, betayRepresenting the system damping term, g representing the target position, y,
Representing the position, velocity, acceleration of the primitive during motion.
3. The method for planning trajectory of a two-arm robot based on dynamic motion primitives and artificial potential fields according to claim 2, wherein the nonlinear forcing terms corresponding to the exemplary path are:
wherein f istargetActual values, y, representing non-linear forcing terms corresponding to exemplary pathsdemo、
Representing the position, velocity and acceleration of the corresponding element of the exemplary trajectory during motion, y0、g0The starting position and the target position corresponding to the exemplary track are shown.
4. The method for planning the trajectory of the dual-arm robot based on the dynamic motion primitives and the artificial potential field according to claim 3, wherein the optimal weight value of each basis function is obtained by a local weighting method, and the method comprises the following steps:
S＝(ξ(1)ξ(2)...ξ(p))T
ξ＝x(g1-y1)
wherein psiiRepresenting a center of compliance as ciVariance is hiThe basis functions of the gaussian distribution of (a),x represents the phase variable, ωiDenotes the weight of the ith basis function, i denotes the subscript, T denotes the transpose, y1、g1Respectively representing the initial position and the target position corresponding to the learning track, and p represents the number and is a set value.
5. The method for planning trajectories of two-arm robots based on dynamic motion primitives and artificial potential fields according to claim 4, wherein the nonlinear forcing term corresponding to the learning path is as follows:
wherein f represents the actual value of the nonlinear forcing term corresponding to the learning path, and N represents the number of the basis functions.
6. The method of claim 5, wherein the acceleration repulsion term is:
U(y)＝Uatt(y)+Urep(y)
wherein,
representing the actual value of the acceleration rejection term, U (y) representing the potential function at point y, Urep(y)、Uatt(y) denotes the gravitational potential, the repulsive potential at point y,
denotes the gravitational gain, d (y, g)1) Representing the current point y to the target point g1η represents the repulsive gain, d (y) represents the distance between point y and the nearest obstacle, and Q represents the obstacle distance action threshold.
7. The method for planning the trajectory of the dual-arm robot based on the dynamic motion primitives and the artificial potential field as claimed in claim 6, wherein the dynamic motion primitive model with the obstacle avoidance function is:
8. a double-arm robot track planning system based on dynamic motion primitives and artificial potential fields is characterized by comprising a position acquisition module and a path planning output module;
the position acquisition module is configured to acquire a target position of the double-arm robot for path planning as input data;
the path planning output module is configured to obtain a corresponding planning path through a pre-constructed dynamic motion primitive model by the double-arm robot based on the input data, and move to the target position along the planning path;
the dynamic motion primitive model is constructed by the following method:
step A10, constructing a double-arm robot model based on a D-H parameter table of the double-arm robot and solving a working space of the double-arm robot through a Monte Carlo algorithm;
step A20, setting a starting position, a target position and a corresponding demonstration path in the working space, and acquiring track motion data of the double-arm robot when moving along the demonstration path in real time as first data; the track motion data comprises displacement, speed and acceleration;
step A30, based on the first data, obtaining a nonlinear forcing term corresponding to the demonstration path through a pre-constructed first model, as a first forcing term; the first model is a dynamic motion primitive model of discrete motion that does not contain an forcing term;
step A40, based on the first forcing term, combining the initial position and the target position of the set learning path, obtaining the optimal weight value of each basis function through a local weighting method, and constructing a nonlinear forcing term corresponding to the learning path as a second forcing term;
step A50, adding the second forcing term and the pre-constructed acceleration repulsion term into the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as a finally constructed dynamic motion primitive model; the acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.
9. A storage means having stored therein a plurality of programs, characterized in that said programs are adapted to be loaded and executed by a processor to implement the method of dual-arm robot trajectory planning based on dynamic motion primitives and artificial potential fields of any of claims 1-7.
10. A processing device comprising a processor, a storage device; a processor adapted to execute various programs; a storage device adapted to store a plurality of programs; characterized in that said program is adapted to be loaded and executed by a processor to implement a method for dual-arm robot trajectory planning based on dynamic motion primitives and artificial potential fields according to any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011399548.XA CN112549028A (en) | 2020-12-02 | 2020-12-02 | Double-arm robot track planning method based on dynamic motion primitives and artificial potential field |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011399548.XA CN112549028A (en) | 2020-12-02 | 2020-12-02 | Double-arm robot track planning method based on dynamic motion primitives and artificial potential field |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112549028A true CN112549028A (en) | 2021-03-26 |
Family
ID=75047995
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011399548.XA Pending CN112549028A (en) | 2020-12-02 | 2020-12-02 | Double-arm robot track planning method based on dynamic motion primitives and artificial potential field |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112549028A (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113043251A (en) * | 2021-04-23 | 2021-06-29 | 江苏理工学院 | Robot teaching reproduction track learning method |
| CN113910236A (en) * | 2021-10-29 | 2022-01-11 | 长安大学 | Method, system, equipment and medium for planning movement of mobile double-arm robot |
| CN115495882A (en) * | 2022-08-22 | 2022-12-20 | 北京科技大学 | Method and device for constructing robot motion primitive library under non-flat terrain |
| CN115524997A (en) * | 2022-09-28 | 2022-12-27 | 山东大学 | Robot dynamic cloth operation method and system based on reinforcement and simulated learning |
| CN116872212A (en) * | 2023-08-14 | 2023-10-13 | 山东工商学院 | Double-mechanical-arm obstacle avoidance planning method based on A-Star algorithm and improved artificial potential field method |
| CN118357931A (en) * | 2024-06-19 | 2024-07-19 | 广东电网有限责任公司潮州供电局 | Path planning method and device for double-arm robot, electronic equipment and storage medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106444738A (en) * | 2016-05-24 | 2017-02-22 | 武汉科技大学 | Mobile robot path planning method based on dynamic motion primitive learning model |
| CN110908373A (en) * | 2019-11-11 | 2020-03-24 | 南京航空航天大学 | Intelligent vehicle track planning method based on improved artificial potential field |
| KR20200072592A (en) * | 2018-12-03 | 2020-06-23 | 한국생산기술연구원 | Learning framework setting method for robot and digital control device |
| CN111736611A (en) * | 2020-07-06 | 2020-10-02 | 中国计量大学 | Mobile robot path planning method based on A-star algorithm and artificial potential field algorithm |
| KR20200130091A (en) * | 2019-05-10 | 2020-11-18 | 한국전자통신연구원 | Method and apparatus for obstacle avoidance path generation of robot using deep learning |
-
2020
- 2020-12-02 CN CN202011399548.XA patent/CN112549028A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106444738A (en) * | 2016-05-24 | 2017-02-22 | 武汉科技大学 | Mobile robot path planning method based on dynamic motion primitive learning model |
| KR20200072592A (en) * | 2018-12-03 | 2020-06-23 | 한국생산기술연구원 | Learning framework setting method for robot and digital control device |
| KR20200130091A (en) * | 2019-05-10 | 2020-11-18 | 한국전자통신연구원 | Method and apparatus for obstacle avoidance path generation of robot using deep learning |
| CN110908373A (en) * | 2019-11-11 | 2020-03-24 | 南京航空航天大学 | Intelligent vehicle track planning method based on improved artificial potential field |
| CN111736611A (en) * | 2020-07-06 | 2020-10-02 | 中国计量大学 | Mobile robot path planning method based on A-star algorithm and artificial potential field algorithm |
Non-Patent Citations (2)
| Title |
|---|
| AUKE JAN IJSPEERT等: "Dynamical Movement Primitives: Learning Attractor Models for Motor Behaviors", 《2013 MASSACHUSETTS INSTITUTE OF TECHNOLOGY》 * |
| DAE-HYUNG PARK等: "Movement reproduction and obstacle avoidance with dynamic movement primitives and potential fields", 《2008 8TH IEEE-RAS INTERNATIONAL CONFERENCE ON HUMANOID ROBOTS》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113043251A (en) * | 2021-04-23 | 2021-06-29 | 江苏理工学院 | Robot teaching reproduction track learning method |
| CN113910236A (en) * | 2021-10-29 | 2022-01-11 | 长安大学 | Method, system, equipment and medium for planning movement of mobile double-arm robot |
| CN113910236B (en) * | 2021-10-29 | 2022-11-29 | 长安大学 | Method, system, equipment and medium for planning movement of mobile double-arm robot |
| CN115495882A (en) * | 2022-08-22 | 2022-12-20 | 北京科技大学 | Method and device for constructing robot motion primitive library under non-flat terrain |
| CN115495882B (en) * | 2022-08-22 | 2024-02-27 | 北京科技大学 | Method and device for constructing robot motion primitive library under uneven terrain |
| CN115524997A (en) * | 2022-09-28 | 2022-12-27 | 山东大学 | Robot dynamic cloth operation method and system based on reinforcement and simulated learning |
| CN115524997B (en) * | 2022-09-28 | 2024-05-14 | 山东大学 | Robot dynamic operation cloth method and system based on reinforcement and imitation learning |
| CN116872212A (en) * | 2023-08-14 | 2023-10-13 | 山东工商学院 | Double-mechanical-arm obstacle avoidance planning method based on A-Star algorithm and improved artificial potential field method |
| CN118357931A (en) * | 2024-06-19 | 2024-07-19 | 广东电网有限责任公司潮州供电局 | Path planning method and device for double-arm robot, electronic equipment and storage medium |
| CN118357931B (en) * | 2024-06-19 | 2024-08-30 | 广东电网有限责任公司潮州供电局 | Path planning method and device for double-arm robot, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112549028A (en) | Double-arm robot track planning method based on dynamic motion primitives and artificial potential field | |
| Chebotar et al. | Actionable models: Unsupervised offline reinforcement learning of robotic skills | |
| Stulp et al. | Hierarchical reinforcement learning with movement primitives | |
| Nguyen-Tuong et al. | Using model knowledge for learning inverse dynamics | |
| Pastor et al. | From dynamic movement primitives to associative skill memories | |
| Ladd et al. | Motion Planning in the Presence of Drift, Underactuation and Discrete System Changes. | |
| Rai et al. | Learning feedback terms for reactive planning and control | |
| Balakrishna et al. | On-policy robot imitation learning from a converging supervisor | |
| Fanger et al. | Gaussian processes for dynamic movement primitives with application in knowledge-based cooperation | |
| Chen et al. | Robot learning from multiple demonstrations with dynamic movement primitive | |
| Torabi et al. | Sample-efficient adversarial imitation learning from observation | |
| Yuan et al. | Hierarchical dynamic movement primitive for the smooth movement of robots based on deep reinforcement learning | |
| Kim et al. | Learning and generalization of dynamic movement primitives by hierarchical deep reinforcement learning from demonstration | |
| Palm et al. | Particle swarm optimization of potential fields for obstacle avoidance | |
| Mei et al. | Mobile robots path planning based on dynamic movement primitives library | |
| Zhou et al. | Generalizable long-horizon manipulations with large language models | |
| Nicola et al. | Deep reinforcement learning for motion planning in human robot cooperative scenarios | |
| Gayle et al. | Efficient motion planning of highly articulated chains using physics-based sampling | |
| Naderi et al. | Learning physically based humanoid climbing movements | |
| Peters et al. | Learning Operational Space Control. | |
| Claassens | An RRT-based path planner for use in trajectory imitation | |
| Nikitenko et al. | Rrts postprocessing for uncertain environments | |
| Peng et al. | Moving object grasping method of mechanical arm based on deep deterministic policy gradient and hindsight experience replay | |
| Tang et al. | Reinforcement learning for robots path planning with rule-based shallow-trial | |
| Jia et al. | Research and implementation of complex task based on DMP |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210326 |