CN112549028A - Double-arm robot track planning method based on dynamic motion primitives and artificial potential field - Google Patents

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

Trajectory Planning Method of Dual-arm Robot Based on Dynamic Motion Primitives and Artificial Potential Field

technical field

The invention belongs to the field of robot trajectory planning, and in particular relates to a dual-arm robot trajectory planning method, system and device based on dynamic motion primitives and artificial potential fields.

Background technique

Trajectory planning is the basis of manipulator control and one of the key technologies to control the motion of dual-arm robots. It is of great significance to the work efficiency and motion stability of the manipulator. Common trajectory planning algorithms include spline interpolation, rapid exploration random tree (RRT), artificial potential field method, etc. The trajectory planning method based on spline interpolation contains a time constant, which makes multi-DOF trajectory learning difficult; the trajectory generated by statistical-based methods such as rapid exploration of random trees has poor continuity, and path detours and deadlocks may occur in complex environments. Due to the blindness of its search, the number of iterations increases, and the amount of calculation increases greatly, resulting in poor real-time performance and low efficiency; artificial potential field method is a relatively mature and efficient planning method in trajectory planning algorithms, because of its simple mathematical model is widely used, but it is prone to falling into local minima. Based on this, the present invention proposes a trajectory planning method for a dual-arm robot based on dynamic motion primitives and artificial potential fields.

SUMMARY OF THE INVENTION

In order to solve the above problems in the prior art, that is, in order to solve the problems of poor real-time performance, poor generalization and low efficiency of the existing robot trajectory planning methods, the first aspect of the present invention proposes a method based on dynamic motion primitives and artificial potential fields. A dual-arm robot trajectory planning method, the method includes:

Step S10, obtaining the target position of the dual-arm robot to be planned by the path as input data;

Step S20, based on the input data, the dual-arm robot obtains a corresponding planned path through a pre-built dynamic motion primitive model, and moves to the target position along the planned path;

Wherein, the construction method of the dynamic motion primitive model is:

Step A10, based on the D-H parameter table of the dual-arm robot, construct a dual-arm robot model and solve the workspace of the dual-arm robot through a Monte Carlo algorithm;

Step A20, set the starting position, the target position and the corresponding demonstration path in the workspace, and collect the trajectory motion data when the dual-arm robot moves along the demonstration path in real time, as the first data; the trajectory motion data Including displacement, velocity, acceleration;

Step A30, based on the first data, obtain a nonlinear forcing term corresponding to the demonstration path through a pre-built first model, as the first forcing term; the first model is a dynamic motion of discrete motion that does not include the forcing term primitive model;

Step A40, based on the first forcing term, combined with the starting position and target position of the set learning path, obtain the optimal weight value of each basis function through a local weighting method, and construct a nonlinear corresponding to the learning path. compulsion, as a second compulsion;

Step A50, adding the second forced term and the pre-built acceleration exclusion term to the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as the final dynamic motion primitive model; the 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 forced terms is:

表示基元在运动过程中的位置、速度、加速度。Among them, τ represents the time scaling factor, α _y represents the elastic constant, β _y represents the system damping term, g represents the target position, y,

Indicates the position, velocity, and acceleration of the primitive during motion.

In some preferred embodiments, the nonlinear forcing term corresponding to the exemplary path is:

表示示范轨迹对应的基元在运动过程中位置、速度和加速度，y₀、g₀表示示范轨迹对应的起始位置、目标位置。Among them, f _target represents the actual value of the nonlinear forced term corresponding to the demonstration path, y _demo ,

It represents the position, velocity and acceleration of the primitive corresponding to the demonstration trajectory during the movement process, and y ₀ and g ₀ represent the starting position and target position corresponding to the demonstration trajectory.

In some preferred embodiments, "obtaining the optimal weight value of each basis function through a local weighting method", the method is:

S=(ξ(1) ξ(2) ... ξ(p)) ^T

ξ=x(g ₁ -y ₁ )

Among them, ψ _i represents the basis function that obeys the Gaussian distribution whose center is c _i and the variance is _hi , S ^T represents, Γ _i represents, x represents the phase variable, ω _i represents the weight of the ith basis function, and i represents the subscript , T represents the transposition, y ₁ and g ₁ represent the starting position and target position corresponding to the learning trajectory, respectively, and p represents the quantity, which is the set value.

In some preferred embodiments, the nonlinear forced term corresponding to the learning path is:

Among them, f represents the actual value of the nonlinear forced term corresponding to the learning path, and N represents the number of basis functions.

In some preferred embodiments, the acceleration exclusion term is:

U(y)=U _att (y)+U _rep (y)

表示加速度排斥项对应的实际值，U(y)表示在点y处的势函数，U_rep(y)、U_att(y)表示在点y处的引力势、斥力势，

表示引力增益，d(y，g₁)表示当前点y到目标点g₁之间的距离，η表示斥力增益，D(y)表示点y与最近障碍物之间的距离，Q表示障碍物距离作用阈值。in,

represents the actual value corresponding to the acceleration repulsion term, U(y) represents the potential function at point y, U _rep (y), U _att (y) represent the gravitational potential and repulsion potential at point y,

represents the gravitational gain, d(y, g ₁ ) represents the distance between the current point y and the target point g ₁ , η represents the repulsion gain, D(y) represents the distance between the point y and the nearest obstacle, and Q represents the obstacle Distance action threshold.

In some preferred embodiments, the dynamic motion primitive model with obstacle avoidance function is:

In a second aspect of the present invention, a dual-arm robot trajectory planning system based on dynamic motion primitives and artificial potential fields is proposed, the system includes a position acquisition module and a path planning output module;

The position obtaining module is configured to obtain the target position of the dual-arm robot to be planned by the path as input data;

The path planning output module is configured to, based on the input data, the dual-arm robot obtains a corresponding planned path through a pre-built dynamic motion primitive model, and moves to the target position along the planned path;

Wherein, the construction method of the dynamic motion primitive model is:

Step A10, based on the D-H parameter table of the dual-arm robot, construct a dual-arm robot model and solve the workspace of the dual-arm robot through a Monte Carlo algorithm;

Step A20, set the starting position, the target position and the corresponding demonstration path in the workspace, and collect the trajectory motion data when the dual-arm robot moves along the demonstration path in real time, as the first data; the trajectory motion data Including displacement, velocity, acceleration;

Step A30, based on the first data, obtain a nonlinear forcing term corresponding to the demonstration path through a pre-built first model, as the first forcing term; the first model is a dynamic motion of discrete motion that does not include the forcing term primitive model;

Step A40, based on the first forcing term, combined with the starting position and target position of the set learning path, obtain the optimal weight value of each basis function through a local weighting method, and construct a nonlinear corresponding to the learning path. compulsion, as a second compulsion;

Step A50, adding the second forced term and the pre-built acceleration exclusion term to the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as the final dynamic motion primitive model; the 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, and the programs are suitable for being loaded and executed by a processor to realize the above-mentioned dual-arm robot trajectory based on dynamic motion primitives and artificial potential fields. planning method.

In a fourth aspect of the present invention, a processing device is proposed, including a processor and a storage device; the processor is adapted to execute various programs; the storage device is adapted to store multiple programs; the programs are adapted to be loaded by the processor And execute to realize the above-mentioned dual-arm robot trajectory planning method based on dynamic motion primitives and artificial potential field.

Beneficial effects of the present invention:

The invention improves the real-time performance, generalization and efficiency of robot trajectory planning.

(1) In the present invention, only one trajectory sample data is collected for training, and the weight parameters of the dynamic motion primitive model are obtained, thereby realizing the autonomous trajectory planning of the robot. On this basis, when the robot completes a new task, as long as the starting point, target point and related parameters of the movement are modified, it can maintain the characteristics of the original sample trajectory to reach the new target position without retraining the sample, and the learning model Rather than targeting a specific task, it has the ability to generalize.

(2) The present invention applies the artificial potential field method to the method of dynamic motion primitives, that is, the repulsive acceleration term is introduced into the second-order dynamic system with stable properties, so that the generated trajectory points are far away from obstacles, and the trajectory thus generated is It not only has the expected movement trend, but also achieves the effect of obstacle avoidance at the same time, which improves the efficiency and safety of trajectory planning.

(3) The present invention applies the artificial potential field method to the method of dynamic motion primitives. Compared with methods based on probability sampling such as rapid exploration of random trees, the trajectory learned by the method of the present invention is more continuous; It is easy to realize multi-degree-of-freedom coupling, which can imitate and learn multi-degree-of-freedom trajectories, and this method has fewer related parameters, and it is easier to adjust parameters.

Description of drawings

Other features, objects and advantages of the present application will become more apparent upon reading the detailed description of non-limiting embodiments taken with reference to the following drawings.

1 is a schematic flowchart of a construction process of a dynamic motion primitive model according to an embodiment of the present invention;

2 is a schematic diagram of a framework of a dual-arm robot trajectory planning system based on dynamic motion primitives and artificial potential fields according to an embodiment of the present invention;

FIG. 3 is an exemplary diagram illustrating the comparison of the generated trajectory and the trajectories of the exemplary trajectory according to an embodiment of the present invention.

Detailed ways

In order to make the objectives, 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. Obviously, the described embodiments are part of the embodiments of the present invention, not All examples. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

The present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the related invention, but not to limit the invention. In addition, it should be noted that, for the convenience of description, only the parts related to the related invention are shown in the drawings.

It should be noted that the embodiments in the present application and the features of the embodiments may be combined with each other in the case of no conflict.

The dual-arm robot trajectory planning method based on dynamic motion primitives and artificial potential fields of the present invention includes the following steps:

Step S10, obtaining the target position of the dual-arm robot to be planned by the path as input data;

Step S20, based on the input data, the dual-arm robot obtains a corresponding planned path through a pre-built dynamic motion primitive model, and moves to the target position along the planned path;

Wherein, the construction method of the dynamic motion primitive model is:

Step A10, based on the D-H parameter table of the dual-arm robot, construct a dual-arm robot model and solve the workspace of the dual-arm robot through a Monte Carlo algorithm;

Step A20, set the starting position, the target position and the corresponding demonstration path in the workspace, and collect the trajectory motion data when the dual-arm robot moves along the demonstration path in real time, as the first data; the trajectory motion data Including displacement, velocity, acceleration;

Step A30, based on the first data, obtain a nonlinear forcing term corresponding to the demonstration path through a pre-built first model, as the first forcing term; the first model is a dynamic motion of discrete motion that does not include the forcing term primitive model;

Step A40, based on the first forcing term, combined with the starting position and target position of the set learning path, obtain the optimal weight value of each basis function through a local weighting method, and construct a nonlinear corresponding to the learning path. compulsion, as a second compulsion;

Step A50, adding the second forced term and the pre-built acceleration exclusion term to the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as the final dynamic motion primitive model; the The acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.

In order to more clearly describe the dual-arm robot trajectory planning method based on dynamic motion primitives and artificial potential fields of the present invention, each step in an embodiment of the method of the present invention will be described in detail below with reference to the accompanying drawings.

In the following embodiments, the construction process of the dynamic motion primitive model is first described in detail, and then the process of obtaining the planned path by the dual-arm robot trajectory planning method based on the dynamic motion primitive and the artificial potential field is described in detail.

1. The construction process of the dynamic motion primitive model, as shown in Figure 1

Step A10, based on the D-H parameter table of the dual-arm robot, construct a dual-arm robot model and solve the workspace of the dual-arm robot through a Monte Carlo algorithm;

In this embodiment, according to the D-H parameter table of the dual-arm robot, a dual-arm robot model is established through the Robotic Toolbox toolkit in MATLAB, and the Monte Carlo algorithm is used to solve and calculate the workspace of the dual-arm robot to obtain the specific working range of the robot. (i.e. workspace).

Step A20, set the starting position, the target position and the corresponding demonstration path in the workspace, and collect the trajectory motion data when the dual-arm robot moves along the demonstration path in real time, as the first data; the trajectory motion data Including displacement, velocity, acceleration;

In this embodiment, the dual-arm robot is turned on, the starting position and the target position are set in the working space of the dual-arm robot, and one is selected so that the dual-arm robot can move from the starting position to the target position and avoid obstacles. The optimal path is the demonstration path.

During the movement of the dual-arm robot, the trajectory motion data along the x-y-z three-dimensional direction in the Cartesian coordinate system are collected in real time when the dual-arm robot moves along the demonstration path. The trajectory motion data includes displacement, velocity, and acceleration.

Step A30, based on the first data, obtain a nonlinear forcing term corresponding to the demonstration path through a pre-built first model, as the first forcing term; the first model is a dynamic motion of discrete motion that does not include the forcing term primitive model;

In this embodiment, the parameters of the dynamic motion primitive model are obtained by using the demonstration trajectory samples for training, so as to realize the autonomous trajectory planning of the robot.

The dynamic motion primitive model DMP is essentially a nonlinear function term that introduces a series of Gaussian functions weighted and superimposed in a stable second-order dynamic system. The motion process of the dynamic system is determined by the nonlinear function, so that the system can reach the target attractor state. The method is based on the spring-mass-damper model, which is abstracted as a point attraction subsystem. Among them, the dynamic motion primitive model of discrete motion without forced terms is shown in formula (1):

表示基元在运动过程中的位置、速度、加速度。Among them, τ represents the time scaling factor, which is used to adjust the scaling speed of the typical system, α _y represents the elastic constant, β _y represents the system damping term, generally β _y = α _y /4, so that the system reaches the critical damping state, g represents the target position, y,

Indicates the position, velocity, and acceleration of the primitive during motion.

The dynamic system in equation (1) is a second-order system with only one point attractor, so the system can only converge to the target position g in a specific motion form. At the target point, we introduce the nonlinear forcing term f into equation (1) to obtain equations (2) and (3):

为x的一阶导数。Among them, formula (3) is the canonical system of modulation time, x represents the phase variable, α _x represents the canonical system parameter,

is the first derivative of x.

The initial parameters set in the above scheme include target position g, elastic constant α _y , system damping term β _y , time scaling factor τ, f is a nonlinear forcing term, so that the system can generate arbitrary nonlinear complex motion. According to equation (2), the nonlinear forcing term in the demonstration trajectory can be derived backwards, as shown in equation (4):

表示示范轨迹对应的基元在运动过程中位置、速度和加速度，y₀、g₀表示示范轨迹对应的起始位置、目标位置。Among them, f _target represents the actual value of the nonlinear forced term corresponding to the demonstration path, y _demo ,

It represents the position, velocity and acceleration of the primitive corresponding to the demonstration trajectory during the movement process, and y ₀ and g ₀ represent the starting position and target position corresponding to the demonstration trajectory.

Step A40, based on the first forcing term, combined with the starting position and target position of the set learning path, obtain the optimal weight value of each basis function through a local weighting method, and construct a nonlinear corresponding to the learning path. compulsion, as a second compulsion;

In this embodiment, the relevant initial parameters of the dynamic motion primitive method are set according to the initial task of the robot, and the starting position and target position of the robot learning and the calculated optimal weight value are given, so that the The dynamic motion primitive model generates a learning trajectory, which has the characteristics of the demonstration trajectory, that is, the motion trend is basically the same as the original demonstration trajectory.

Among them, the nonlinear forced term of the learning trajectory is calculated, as shown in formulas (5) and (6):

Among them, f represents the actual value of the nonlinear forcing term corresponding to the learning path, N represents the number of basis functions, ψ _i represents the basis function obeying a Gaussian distribution with center c _i and variance _hi , x represents the phase variable, ω _i represents the weight of the ith basis function, i represents the subscript, T represents the transposition, and y ₁ and g ₁ represent the starting position and target position corresponding to the learning trajectory.

It can be seen that the basis functions ψ _i are combined into a forcing function f through weighted addition. Since the basis function _ψi is nonlinear, the forcing function f and the entire system of dynamic motion primitives are also nonlinear. Equation (6) shows that the basis function ψ _i obeys a Gaussian distribution whose center is _ci and whose variance is _hi . ω _i is the basis function weight, N is the number of basis functions, the more the number of basis functions, the smoother the generalized target trajectory.

Basis function weight, its calculation process is shown in formula (7)(8)(9)(10):

S=(ξ(1) ξ(2) ... ξ(p)) ^T (8)

ξ=x(g ₁ -y ₁ ) (9)

Among them, ψ _i represents the basis function obeying the Gaussian distribution whose center is c _i and the variance is hi, _i represents the subscript, T represents the transposition, y ₁ and g ₁ represent the corresponding starting position and target position of the learning trajectory, respectively, p represents the quantity, which is the set value.

As shown in FIG. 3 , one demonstration trajectory (black curve) and the other is the learning trajectory (grey curve), and the trajectory has the characteristics of the demonstration trajectory, that is, the movement trend is basically the same as the original demonstration trajectory.

Step A50, adding the second forced term and the pre-built acceleration exclusion term to the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as the final dynamic motion primitive model; the The acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.

Artificial potential field method is a common online obstacle avoidance method. In the present invention, the potential field is defined around the obstacle, and the gradient of the potential field produces a repulsive force on the robot. This approach is common in motion planning for mobile robots and is also used in robotic actuators.

表示加速度排斥项对应的实际值。The present invention focuses on the motion of the end-effector of the manipulator. Compared with the joint space, the position and potential field of obstacles are easier to obtain in the actuator space. Therefore, the use of DMPs in the execution space only needs to add an exclusion term. A dynamic motion primitive model with obstacle avoidance function can be obtained. This addition allows the resulting 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.

Indicates the actual value corresponding to the acceleration exclusion term.

是人工势场的负梯度，它取决于末端执行器相对于障碍物的相对位置和速度，如此便使得生成的运动路径被势函数的属性改变。加入一个加速度排斥项的动态运动基元模型为式(11)：In this 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 relative to the obstacle, so that the resulting motion path is changed by the properties of the potential function. The dynamic motion primitive model with an acceleration repulsion term added is equation (11):

某点y处的势函数U(y)为引力势和斥力势之和，如公式(12)所示：We can set the obstacle position point in the working environment as the repulsion field U _rep (y) according to the actual task needs, and set the task target point as the gravitational field U _att (y), then it will be affected by the total force field U ( y) impact. The additional term in Eq. is 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 formula (12):

U(y)=U _att (y)+U _rep (y) (12)

The gravitational potential function is shown in formula (13):

The repulsive potential function is shown in formula (14):

表示加速度排斥项对应的实际值，U(y)表示在点y处的势函数，U_rep(y)、U_att(y)表示在点y处的引力势、斥力势，

表示引力增益，d(y，g₁)表示当前点y到目标点g₁之间的距离，η表示斥力增益，D(y)表示点y与最近障碍物之间的距离，Q表示障碍物距离作用阈值，大于此距离的障碍物不会产生斥力影响。in,

represents the actual value corresponding to the acceleration repulsion term, U(y) represents the potential function at point y, U _rep (y), U _att (y) represent the gravitational potential and repulsion potential at point y,

represents the gravitational gain, d(y, g ₁ ) represents the distance between the current point y and the target point g ₁ , η represents the repulsion gain, D(y) represents the distance between the point y and the nearest obstacle, and Q represents the obstacle Threshold of distance action. Obstacles larger than this distance will not have repulsion effect.

During the operation of the dual-arm robot, there will be a situation where the distance between the obstacle and the target point is close. In this case, the artificial potential field method will cause the target to be unreachable. In view of this situation, we improve the repulsion function. In the original repulsion function, a relative distance term between the robot and the target point is added, as shown in the above formula (14), so that the gravitational force and the repulsive force approach simultaneously when the robot moves to the target point. is set to 0, so that the resultant force when the robot reaches the target point is 0, and the problem that the target cannot be reached in the artificial potential field method is solved.

In this way, the artificial potential field method is applied to the method of dynamic motion primitives, so that the repulsive acceleration term is included in the second-order system, so that the generated trajectory points are far away from obstacles, so the generated trajectory not only has the expected motion trend, but also can simultaneously achieve the effect of avoiding obstacles.

Based on the constructed dynamic motion primitive model with obstacle avoidance function, the displacement, velocity and acceleration information of different trajectories can be calculated according to the starting positions and target positions of different trajectories, and the characteristics of the original sample trajectories can be maintained without retraining the samples. Reach a new target position, and the process is not specific to a specific task, and has the ability to generalize.

2. Two-arm robot trajectory planning method based on dynamic motion primitives and artificial potential fields

Step S10, obtaining the target position of the dual-arm robot to be planned by the path as input data;

In this embodiment, the target position of the dual-arm robot for path planning is obtained.

Step S20, based on the input data, the dual-arm robot obtains a corresponding planned path through a pre-built dynamic motion primitive model, and moves to the target position along the planned path.

In this embodiment, the dual-arm robot obtains a corresponding planned path through the above constructed dynamic motion primitive model with an obstacle avoidance function, and moves to the target position along the planned path.

A dual-arm robot trajectory planning system based on dynamic motion primitives and artificial potential fields according to the 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 obtaining module 100 is configured to obtain the target position of the dual-arm robot to be planned by the path as input data;

The path planning output module 200 is configured to, based on the input data, the dual-arm robot obtains a corresponding planned path through a pre-built dynamic motion primitive model, and moves to the target position along the planned path;

Wherein, the construction method of the dynamic motion primitive model is:

Step A10, based on the D-H parameter table of the dual-arm robot, construct a dual-arm robot model and solve the workspace of the dual-arm robot through a Monte Carlo algorithm;

Step A20, set the starting position, the target position and the corresponding demonstration path in the workspace, and collect the trajectory motion data when the dual-arm robot moves along the demonstration path in real time, as the first data; the trajectory motion data Including displacement, velocity, acceleration;

Step A30, based on the first data, obtain a nonlinear forcing term corresponding to the demonstration path through a pre-built first model, as the first forcing term; the first model is a dynamic motion of discrete motion that does not include the forcing term primitive model;

Step A40, based on the first forcing term, combined with the starting position and target position of the set learning path, obtain the optimal weight value of each basis function through a local weighting method, and construct a nonlinear corresponding to the learning path. compulsion, as a second compulsion;

Step A50, adding the second forced term and the pre-built acceleration exclusion term to the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as the final dynamic motion primitive model; the The acceleration repulsion term is constructed based on the negative gradient of the artificial potential field.

Those skilled in the technical field can clearly understand that, for the convenience and brevity of description, for the specific working process and related description of the system described above, reference may be made to the corresponding process in the foregoing method embodiments, which will not be repeated here.

It should be noted that the trajectory planning system for a dual-arm robot based on dynamic motion primitives and artificial potential fields provided by the above embodiments is only illustrated by the division of the above functional modules. In practical applications, the above can be used as required. The function allocation is completed by different function modules, that is, the modules or steps in the embodiments of the present invention are decomposed or combined. For example, the modules in the above-mentioned embodiments can be combined into one module, or can be further split into multiple sub-modules to complete All or part of the functions described above. The names of the modules and steps involved in the embodiments of the present invention are only for distinguishing each module or step, and should not be regarded as an improper limitation of the present invention.

A storage device according to the third embodiment of the present invention stores a plurality of programs, which are suitable for being loaded by a processor and implementing the above-mentioned dual-arm robot trajectory planning method based on dynamic motion primitives and artificial potential fields.

A processing device according to a fourth embodiment of the present invention includes a processor and a storage device; the processor is adapted to execute various programs; the storage device is adapted to store multiple programs; the programs are adapted to be loaded and executed by the processor In order to realize the above-mentioned dual-arm robot trajectory planning method based on dynamic motion primitives and artificial potential field.

Those skilled in the technical field can clearly understand that the undescribed convenience and brevity are not described. Repeat.

Those skilled in the art should be aware that the modules and method steps of each example described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, computer software or a combination of the two, and the programs corresponding to the software modules and method steps Can be placed in random access memory (RAM), internal memory, read only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or as known in the art in any other form of storage medium. In order to clearly illustrate the interchangeability of electronic hardware and software, the components and steps of each example have been described generally in terms of functionality in the foregoing description. Whether these functions are performed in electronic hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may use different methods of implementing the described functionality for each particular application, but such implementations should not be considered beyond the scope of the present invention.

The terms "first," "second," etc. are used to distinguish between similar objects, and are not used to describe or indicate a particular order or sequence.

The term "comprising" or any other similar term is intended to encompass a non-exclusive inclusion such that a process, method, article or device/means comprising a list of elements includes not only those elements but also other elements not expressly listed, or Also included are elements inherent to these processes, methods, articles or devices/devices.

So far, the technical solutions of the present invention have been described with reference to the preferred embodiments shown in the accompanying drawings, however, those skilled in the art can easily understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

Claims (10)

1. a dual-arm robot trajectory planning method based on dynamic motion primitive and artificial potential field, is characterized in that, this method comprises the following steps: Step S10, obtaining the target position of the dual-arm robot to be planned by the path as input data; Step S20, based on the input data, the dual-arm robot obtains a corresponding planned path through a pre-built dynamic motion primitive model, and moves to the target position along the planned path; Wherein, the construction method of the dynamic motion primitive model is: Step A10, based on the D-H parameter table of the dual-arm robot, construct a dual-arm robot model and solve the workspace of the dual-arm robot through a Monte Carlo algorithm; Step A20, set the starting position, the target position and the corresponding demonstration path in the workspace, and collect the trajectory motion data when the dual-arm robot moves along the demonstration path in real time, as the first data; the trajectory motion data Including displacement, velocity, acceleration; Step A30, based on the first data, obtain a nonlinear forcing term corresponding to the demonstration path through a pre-built first model, as the first forcing term; the first model is a dynamic motion of discrete motion that does not include the forcing term primitive model; Step A40, based on the first forcing term, combined with the starting position and target position of the set learning path, obtain the optimal weight value of each basis function through a local weighting method, and construct a nonlinear corresponding to the learning path. compulsion, as a second compulsion; Step A50, adding the second forced term and the pre-built acceleration exclusion term to the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as the final dynamic motion primitive model; the The acceleration repulsion term is constructed based on the negative gradient of the artificial potential field. 2. the dual-arm robot trajectory planning method based on dynamic motion primitive and artificial potential field according to claim 1, is characterized in that, the described dynamic motion primitive model that does not comprise the discrete motion of forced term is:

Among them, τ represents the time scaling factor, α _y represents the elastic constant, β _y represents the system damping term, g represents the target position, y,

Indicates the position, velocity, and acceleration of the primitive during motion. 3. the dual-arm robot trajectory planning method based on dynamic motion primitive and artificial potential field according to claim 2, is characterized in that, the nonlinear forced term corresponding to described demonstration path is:

Among them, f _target represents the actual value of the nonlinear forced term corresponding to the demonstration path, y _demo ,

It represents the position, velocity and acceleration of the primitive corresponding to the demonstration trajectory during the movement process, and y ₀ and g ₀ represent the starting position and target position corresponding to the demonstration trajectory. 4. the dual-arm robot trajectory planning method based on dynamic motion primitive and artificial potential field according to claim 3, is characterized in that, " obtains the best weight value of each basis function by local weighting method ", its method is :

S=(ξ(1)ξ(2)...ξ(p)) ^T ξ=x(g ₁ -y ₁ )

Among them, ψ _i represents the basis function obeying the Gaussian distribution with center c _i and variance _hi , x represents the phase variable, ω _i represents the weight of the ith basis function, i represents the subscript, T represents the transpose, y ₁ , g ₁ respectively represent the starting position and target position corresponding to the learning trajectory, and p represents the quantity, which is the set value. 5. The dual-arm robot trajectory planning method based on dynamic motion primitive and artificial potential field according to claim 4, it is characterized in that, the nonlinear forced term corresponding to described learning path is:

Among them, f represents the actual value of the nonlinear forced term corresponding to the learning path, and N represents the number of basis functions. 6. the dual-arm robot trajectory planning method based on dynamic motion primitive and artificial potential field according to claim 5, is characterized in that, described acceleration exclusion term is:

U(y)=U _att (y)+U _rep (y)

in,

represents the actual value corresponding to the acceleration repulsion term, U(y) represents the potential function at point y, U _rep (y), U _att (y) represent the gravitational potential and repulsion potential at point y,

represents the gravitational gain, d(y, g ₁ ) represents the distance between the current point y and the target point g ₁ , η represents the repulsion gain, D(y) represents the distance between the point y and the nearest obstacle, and Q represents the obstacle Distance action threshold. 7. the dual-arm robot trajectory planning method based on dynamic motion primitive and artificial potential field according to claim 6, is characterized in that, described dynamic motion primitive model with obstacle avoidance function is:

8. A dual-arm robot trajectory planning system based on dynamic motion primitives and artificial potential fields, characterized in that the system comprises a position acquisition module and a path planning output module; The position obtaining module is configured to obtain the target position of the dual-arm robot to be planned by the path as input data; The path planning output module is configured to, based on the input data, the dual-arm robot obtains a corresponding planned path through a pre-built dynamic motion primitive model, and moves to the target position along the planned path; Wherein, the construction method of the dynamic motion primitive model is: Step A10, based on the D-H parameter table of the dual-arm robot, construct a dual-arm robot model and solve the workspace of the dual-arm robot through a Monte Carlo algorithm; Step A20, set the starting position, the target position and the corresponding demonstration path in the workspace, and collect the trajectory motion data when the dual-arm robot moves along the demonstration path in real time, as the first data; the trajectory motion data Including displacement, velocity, acceleration; Step A30, based on the first data, obtain a nonlinear forcing term corresponding to the demonstration path through a pre-built first model, as the first forcing term; the first model is a dynamic motion of discrete motion that does not include the forcing term primitive model; Step A40, based on the first forcing term, combined with the starting position and target position of the set learning path, obtain the optimal weight value of each basis function through a local weighting method, and construct a nonlinear corresponding to the learning path. compulsion, as a second compulsion; Step A50, adding the second forced term and the pre-built acceleration exclusion term to the first model, and constructing a dynamic motion primitive model with an obstacle avoidance function as the final dynamic motion primitive model; the The acceleration repulsion term is constructed based on the negative gradient of the artificial potential field. 9. A storage device, wherein a plurality of programs are stored, wherein the programs are adapted to be loaded and executed by a processor to realize the dynamic motion primitive-based and artificial potential according to any one of claims 1-7 Field-based dual-arm robot trajectory planning method. 10. A processing device, comprising a processor and a storage device; the processor is adapted to execute each program; the storage device is adapted to store a plurality of programs; characterized in that the program is adapted to be loaded and executed by the processor to The method for planning the trajectory of a dual-arm robot based on dynamic motion primitives and artificial potential fields according to any one of claims 1 to 7 is realized.

Images