Real-time skill trajectory generation method based on dynamic motion primitive and artificial potential field method

Abstract

The application discloses a real-time skill trajectory generation method based on dynamic motion primitives and artificial potential field method, comprising the following steps: step 1, a group of object trajectories are collected as demonstration trajectories, and trajectory information is obtained; step 2, based on the trajectory information, the demonstration trajectory is learned by using a dynamic motion primitive algorithm; step 3, the next moment corresponding to the task object coordinate system origin position is generated according to the target object position and the current moment task object coordinate system origin position; step 4, collision detection is performed on the generated next moment task object coordinate system origin position, if collision occurs, an obstacle avoidance gradient is generated according to the artificial potential field method, the next trajectory point position is adjusted until no collision occurs, otherwise, the step 3 is returned until the task is completed. The method can imitate learning of the skill demonstration trajectory, encode important trajectory features of the skill, avoid unnecessary mirror ability, reduce excessive amplification of the trajectory, and improve the end trajectory similarity.

Description

Technical Field

[0001] The present invention belongs to the field of machine skill imitation learning and relates to a real-time skill trajectory generation method based on dynamic motion primitives and artificial potential field method. Background Art

[0002] With the development of technology, robots are gradually being used in the service industry. Traditionally, skills are mostly achieved through manual programming, which is time-consuming and labor-intensive, and requires reprogramming whenever the scenario or task objectives change. Therefore, imitation learning methods, which learn skill strategies from human demonstrations, have gained popularity. Skills derived from imitation learning can adapt to different scenarios and target states, adaptively generating the required skill trajectories to complete the execution of skill sequences.

[0003] Common skill learning algorithms, such as the Dynamic Movement Primitives (DMP) algorithm, have good generalization capabilities. However, when the starting and target points in the demonstration trajectory are close, subsequent generalization may cause over-amplification problems. DMP also causes trajectory mirroring problems and end-trajectory similarity issues. Summary of the Invention

[0004] To address the above technical problems in the prior art, the present invention proposes a real-time skill trajectory generation method based on dynamic motion primitives and artificial potential field method, which avoids unnecessary mirroring capabilities, reduces excessive trajectory amplification, and improves the similarity of terminal trajectories. The specific technical solution is as follows:

[0005] A real-time skill trajectory generation method based on dynamic motion primitives and artificial potential field method, comprising:

[0006] Step 1: Collect a set of object trajectories as demonstration trajectories;

[0007] Step 2: Use the dynamic motion primitive algorithm to learn the demonstration trajectory;

[0008] Step 3: Generate the origin position of the task object coordinate system corresponding to the next moment in real time based on the target object position and the origin position of the task object coordinate system at the current moment;

[0009] Step 4: Perform collision detection on the origin position of the generated task object coordinate system at the next moment. If a collision occurs, generate an obstacle avoidance gradient based on the artificial potential field method and adjust the next trajectory point until there is no collision. Otherwise, return to step 3 until the task is completed.

[0010] Furthermore, in step 1, an object coordinate system is constructed based on the key points of the object, and the object coordinate system information at each moment is collected to form a trajectory to represent the skill. The trajectory is expressed in the robot base coordinate system. The expression of the demonstration trajectory is as follows:

[0011]

[0012] ξ i ={p i ,q i},

[0013]

[0014]

[0015]

[0016]

[0017] Where i represents the index of the trajectory point, T is the total number of trajectory points, and t i represents the time corresponding to the i-th trajectory point; ξ i ={p i ,q i} represents the pose trajectory point, including position information p = [x, y, z] T and posture information q=[q0,q1,q2,q3] T , x, y, z represent the three-dimensional coordinates corresponding to the origin of the object coordinate system, q0, q1, q2, q3 represent the unit quaternion corresponding to the rotation matrix of the object coordinate system; Represents the derivative of the pose trajectory point with respect to time, including the position derivative ω i represents the angular velocity, dt is the time interval between adjacent trajectory points, norm([q1,q2,q3] T ) represents the modulus of the vector [q1,q2,q3]; Represents the second-order derivative of the pose trajectory point with respect to time, including the second-order derivative of position angular acceleration ξ goal ={p goal ,q goal} represents the pose data of the target object coordinate system, and its definition form is consistent with the pose trajectory point.

[0018] Furthermore, in step 2, the trajectory shape is weightedly learned by calculating the weight of the nonlinear term in the dynamic motion primitive algorithm formula.

[0019] Furthermore, the weight of the nonlinear term is calculated. The specific expression is as follows:

[0020]

[0021]

[0022]

[0023]

[0024]

[0025] Where N represents the number of basis functions in the nonlinear term, ψ j (s) represents the jth basis function, w j is the weight corresponding to the basis function, s i is the phase corresponding to time i, τ is the total duration of the demonstration trajectory, c j represents the center of the Gaussian basis function, h j represents the width of the Gaussian basis function, and α is the speed adjustment coefficient;

[0026] Construct the following matrix:

[0027]

[0028]

[0029]

[0030] Where φ is the basis function matrix, F is the nonlinear term matrix, and W is the weight matrix, then the following formula is obtained:

[0031] φ*W=F,

[0032] In the above formula, the basis function matrix φ is directly determined by the prior parameter definition, and the nonlinear term matrix F is calculated by the demonstration trajectory data. The calculation formula is as follows:

[0033] The formula for the nonlinear term in the x-direction is as follows:

[0034]

[0035] where α x , β x is the second-order system parameter, x i is the x coordinate of the demonstration trajectory at time i, x goal is the x-coordinate of the origin of the target coordinate system;

[0036] The formula for the nonlinear term in the y direction is as follows:

[0037]

[0038] where α y, β y is the second-order system parameter, y i is the y coordinate of the demonstration trajectory at time i, y goal is the y coordinate of the origin of the target coordinate system;

[0039] The formula for the nonlinear term in the z direction is as follows:

[0040]

[0041] z structure,i =z i -z0*f control,i -z goal *(1-f control,i ),

[0042]

[0043] where α z , β z is the second-order system parameter, z i is the z coordinate of the demonstration trajectory at time i, z0 is the z coordinate of the demonstration trajectory at the initial moment, z goal is the z coordinate of the origin of the target coordinate system, f control,i is the control coefficient at time i;

[0044] After obtaining the F matrix, the approximate solution of the weight matrix W is further obtained by the pseudo-inverse method:

[0045] W=(φ T φ) -1 φ T F.

[0046] Furthermore, in step 3, the origin position of the task object coordinate system at the next moment is obtained by calculating the coordinate values ​​in the x, y, and z directions. The calculation expression for the value in the x direction at the next moment is:

[0047]

[0048]

[0049]

[0050]

[0051]

[0052]

[0053] In the above formula, x newgoal is the x coordinate of the origin of the new target object coordinate system, x newstartis the x coordinate of the origin of the new starting object coordinate system, x goal ,x0 are the target x coordinate and the starting x coordinate in the demonstration trajectory, t c is a manually selected moment, the subsequent trajectory will be close to the demonstration trajectory, k controlx (t) is the generated control parameter, is its derivative with respect to time, x new,i is the x coordinate of the origin of the current object coordinate system, is its derivative with respect to time, is the second-order derivative with respect to time;

[0054] To calculate the value of the y direction at the next moment, replace the above x parameter with the parameter corresponding to the y direction;

[0055] The calculation expression of the value at the next moment in the z direction is:

[0056]

[0057]

[0058]

[0059]

[0060] z new,i+1 =z newstructure,i+1 +z newstart *f controlz,i +z newgoal *(1-f controlz,i ),

[0061]

[0062] where z newstructure,i , The new z coordinate constructed at time i and the corresponding first-order and second-order time derivatives, z new,i+1 The z coordinate of the origin of the object coordinate system actually generated at time i+1.

[0063] Furthermore, the step 4 specifically includes:

[0064] Calculate the shortest distance D(o) between the object and the obstacle at the i+1th moment i+1 ), o i+1 Indicates the object coordinate system information at the i+1th moment: o i+1 =[x new,i+1 ,y new,i+1 ,z new,i+1 ];

[0065] The collision detection is judged as D(oi+1 )<Q * When the obstacle avoidance gradient is calculated, the object coordinate system information o at the i+1th moment is updated. i+1 To adjust the position of the trajectory point, the object coordinate system information o at the i+1th moment is updated i+1 The expression is as follows:

[0066] o i+1 =o i+1 -F(o i+1 )*step,

[0067] Where step is compensation, Q * is the obstacle influence range, F(o i+1 ) is the obstacle avoidance gradient;

[0068] After the update is completed, the closest distance D(o i+1 );

[0069] The collision detection is judged as D(o i+1 )>Q * If , it ends and returns to step 3, otherwise it recalculates the obstacle avoidance gradient according to the above formula and continues to update o i+1 .

[0070] Furthermore, the obstacle avoidance gradient is calculated by the artificial potential field method, and the expression is as follows:

[0071]

[0072]

[0073] F att (o i+1 )=-▽U att (o i+1 )=-ζ(o i+1 -o goal ),

[0074]

[0075]

[0076] Where ζ is the target attraction coefficient, ρ(o i+1 ,o goal ) is the modulus of the difference between two vectors, o goal is the origin coordinate of the target coordinate system, U att (o i+1 ) is the target attraction potential field, η is the obstacle potential field coefficient, U rep (o i+1 ) is the obstacle avoidance potential field, F att (oi+1 ) is U att (o i+1 ) with respect to the gradient in the x, y, and z directions, F rep (o i+1 ) is U rep (o i+1 ) with respect to the gradient in the x, y, and z directions, ▽D(o i+1 ) is D(o i+1 ) with respect to the gradient in the x, y, and z directions.

[0077] The advantages and beneficial effects of the present invention are:

[0078] The method of the present invention can perform imitation learning on the skill demonstration trajectory to encode the important trajectory characteristics of the skill, avoid unnecessary mirroring capabilities, reduce excessive amplification of the trajectory, and improve the similarity of the terminal trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 This is a flow chart of the real-time skill trajectory generation method based on dynamic motion primitives and artificial potential field method of the present invention. DETAILED DESCRIPTION

[0080] In order to make the purpose, technical solution and technical effect of the present invention more clear, the present invention is further described in detail below with reference to the accompanying drawings and embodiments.

[0081] like Figure 1 As shown, the real-time skill trajectory generation method based on dynamic motion primitives and artificial potential field method of the present invention includes the following steps:

[0082] Step 1: Collect a set of object trajectories as demonstration trajectories. The information of the demonstration trajectory includes: the position, velocity, and acceleration of the origin of the object coordinate system at each moment, the unit quaternion corresponding to the rotation matrix of the object coordinate system at each moment, and the position and orientation of the target object coordinate system.

[0083] Specifically, an object coordinate system is constructed based on the key points of the object. The object coordinate system information at each moment is collected to form a trajectory to represent the skill. The trajectory is expressed in the robot base coordinate system. The expression of the skill demonstration trajectory represented by time, position, velocity, acceleration, quaternion posture, angular velocity, and angular acceleration information is as follows:

[0084]

[0085] ξ i ={p i ,q i},

[0086]

[0087]

[0088]

[0089]

[0090] Where i represents the index of the trajectory point, T is the total number of trajectory points, and t i represents the time corresponding to the i-th trajectory point; ξ i ={p i ,q i} represents the pose trajectory point, including position information p = [x, y, z] T and posture information q=[q0,q1,q2,q3] T , x, y, z represent the three-dimensional coordinates corresponding to the origin of the object coordinate system, q0, q1, q2, q3 represent the unit quaternion corresponding to the rotation matrix of the object coordinate system; Represents the derivative of the pose trajectory point with respect to time, including the position derivative ω i represents the angular velocity, dt is the time interval between adjacent trajectory points, norm([q1,q2,q3] T ) represents the modulus of the vector [q1,q2,q3]; Represents the second-order derivative of the pose trajectory point with respect to time, including the second-order derivative of position angular acceleration ξ goal ={p goal ,q goal} represents the pose data of the target object coordinate system, and its definition form is consistent with the pose trajectory point.

[0091] Step 2: Based on the trajectory information obtained above, the dynamic motion primitive algorithm is used to learn the demonstration trajectory.

[0092] Specifically, calculate the weight of the nonlinear term in the dynamic motion primitive formula:

[0093] The nonlinear term f at the i-th moment i The expression is as follows:

[0094]

[0095]

[0096]

[0097]

[0098]

[0099] Where N represents the number of basis functions in the nonlinear term, ψ j (s) represents the jth basis function, w j is the weight corresponding to the basis function, s i is the phase corresponding to time i, τ is the total duration of the demonstration trajectory, c j represents the center of the Gaussian basis function, h j represents the width of the Gaussian basis function, and α is the speed adjustment coefficient. In this embodiment, N is 350, τ is 661*0.01, and α is 48.

[0100] Construct the following matrix:

[0101]

[0102]

[0103]

[0104] Where φ is the basis function matrix, F is the nonlinear term matrix, and W is the weight matrix, then the following formula can be obtained:

[0105] φ*W=F,

[0106] In the above formula, the basis function matrix φ can be directly determined by the prior parameter definition, and the nonlinear term matrix F needs to be calculated through the demonstration trajectory data. The calculation formula is as follows:

[0107] The formula for the nonlinear term in the x-direction is as follows:

[0108]

[0109] where α x , β x is the second-order system parameter, x i is the x coordinate of the demonstration trajectory at time i, x goal is the x-coordinate of the origin of the target coordinate system;

[0110] The formula for the nonlinear term in the y direction is as follows:

[0111]

[0112] where α y , β y is the second-order system parameter, y i is the y coordinate of the demonstration trajectory at time i, y goal is the y coordinate of the origin of the target coordinate system;

[0113] The formula for the nonlinear term in the z direction is as follows:

[0114]

[0115] z structure,i = z i - z0*f control,i - z goal *(1-f control,i ),

[0116]

[0117] wherein α z , β z are second-order system parameters, z i is the demonstration trajectory z coordinate at i moment, z0 is the initial moment demonstration trajectory z coordinate, z goal is the target coordinate system origin z coordinate, f control,i is the control coefficient at i moment.

[0118] In this embodiment, α x = α y = α z = 48, β x = β y = β z = 12, after obtaining the F matrix, the W approximate solution can be further obtained by pseudo-inverse method:

[0119] W = (φ T φ) -1 φ T F.

[0120] Step 3: Real-time according to the target object position and the current moment task object coordinate system origin position, the next moment corresponding task object coordinate system origin position is generated.

[0121] Wherein, the generation mode of x direction and y direction is similar, taking x as an example, the value of x direction at next moment is:

[0122]

[0123]

[0124]

[0125]

[0126]

[0127]

[0128] Wherein x newgoal is the new target object coordinate system origin x coordinate, x newstart is the new starting object coordinate system origin x coordinate, x goal,x0 are the target x coordinate and the starting x coordinate in the demonstration trajectory, t c is a manually selected moment, the subsequent trajectory will be close to the demonstration trajectory, k controlx (t) is the generated control parameter, is its derivative with respect to time, x new,i is the x coordinate of the origin of the current object coordinate system, is its derivative with respect to time, is the second-order derivative with respect to time, and other parameters not mentioned are the same as before.

[0129] For the y direction, just replace the above x parameters with the parameters corresponding to the y direction.

[0130] The next moment value in the z direction:

[0131]

[0132]

[0133]

[0134]

[0135] z new,i+1 =z newstructure,i+1 +z newstart *f controlz,i +z newgoal *(1-f controlz,i ),

[0136]

[0137] where z newstructure,i , The new z coordinate constructed at time i and the corresponding first-order and second-order time derivatives, z new,i+1 The z coordinate of the origin of the object coordinate system actually generated at time i+1. Other parameters not mentioned are the same as before.

[0138] Step 4: Perform collision detection on the origin position of the generated task object coordinate system at the next moment. If a collision occurs, calculate the obstacle avoidance gradient according to the artificial potential field method and adjust the next trajectory point until there is no collision. Otherwise, return to step 3 until the task is completed.

[0139] After step 3 is completed, the object coordinate system x at the i+1th moment will be obtained. new,i+1 ,y new,i+1 ,z new,i+1 Coordinates, based on which the object coordinate system at time i+1 is generated, using o i+1 Indicates the object coordinate system information at the i+1th moment: o i+1= [x new,i+1 ,y new,i+1 ,z new,i+1 ].

[0140] Specifically, the o i+1 object coordinate system state at time i+1 is calculated, and the closest distance D(o i+1 ) of the current object to the obstacle is calculated. i+1 If D(o * ) < Q att , the obstacle avoidance gradient is calculated, otherwise, step 3 is returned until the task is completed.

[0141]

[0142]

[0143] F att (o i+1 ) = -▽U att (o +1 ) = -ζ(o i+1 -o goal ),

[0144]

[0145]

[0146] where ζ is a target attraction coefficient, ρ(o i+1 ,o goal ) is the modulus of the difference between two vectors, o goal is the coordinate of the origin of the target coordinate system, U att (o i+1 ) is the target attraction potential field, Q * is the influence range of the obstacle, η is the obstacle potential field coefficient, U rep (o i+1 ) is the obstacle avoidance potential field, F att (o i+1 ) is the gradient of U att (o i+1 ) with respect to x, y, and z, F rep (o i+1 ) is the gradient of U rep (o i+1 ) with respect to x, y, and z, and ▽D(o i+1 ) is the gradient of D(o i+1 ) with respect to x, y, and z.

[0147] After calculating the obstacle avoidance gradient F(o i+1 ), the object coordinate system information o i+1 at time i+1 is updated as follows:

[0148] o i+1 =o i+1 -F(o i+1 )*step,

[0149] Where step is compensation, and after the update is completed, the closest distance D(o i+1 );

[0150] If D(o i+1 )>Q * Then return to step 3, otherwise recalculate F(o i+1 ) and continue to update o i+1 .

[0151] The above description is only a preferred embodiment of the present invention and does not constitute any form of limitation to the present invention. Although the implementation process of the present invention is described in detail above, it is still possible for those familiar with the art to modify the technical solutions described in the above examples or to replace some of the technical features therein with equivalents. Any modifications, equivalent replacements, etc. made within the spirit and principles of the present invention shall be included in the scope of protection of the present invention.

Claims

1. A real-time skill trajectory generation method based on dynamic motion primitives and artificial potential field method, characterized in that: include: Step 1: Collect a set of object trajectories as demonstration trajectories; Step 2: Use the dynamic motion primitive algorithm to learn the demonstration trajectory; Step 3: Generate the origin position of the task object coordinate system corresponding to the next moment in real time based on the target object position and the origin position of the task object coordinate system at the current moment; Step 4: Perform collision detection on the origin of the generated task object coordinate system at the next moment. If a collision occurs, generate an obstacle avoidance gradient based on the artificial potential field method and adjust the next trajectory point until there is no collision. Otherwise, return to step 3 until the task is completed. In step 1, an object coordinate system is constructed based on the key points of the object, and the object coordinate system information at each moment is collected to form a trajectory to represent the skill. The trajectory is expressed in the robot base coordinate system. The expression of the demonstration trajectory is as follows: ， ， ， ， ， ， in i represents the trajectory point index of the trajectory, T is the total number of trajectory points, Indicates the i The time corresponding to each trajectory point; Represents the pose trajectory point, including position information and posture information , x , y , z Represents the three-dimensional coordinates corresponding to the origin of the object coordinate system, Represents the unit quaternion corresponding to the rotation matrix of the object coordinate system; Represents the derivative of the pose trajectory point with respect to time, including the position derivative , represents the angular velocity, is the time interval between adjacent trajectory points, express The magnitude of a vector; Represents the second-order derivative of the pose trajectory point with respect to time, including the second-order derivative of position , angular acceleration ; The pose data of the target object coordinate system is defined in the same format as the pose trajectory point.

2. The real-time skill trajectory generation method according to claim 1, characterized in that: In step 2, weighted learning of the trajectory shape is performed by calculating the weight of the nonlinear term in the dynamic motion primitive algorithm formula.

3. The real-time skill trajectory generation method according to claim 2, characterized in that: Calculate the weight of the nonlinear term. The specific expression is as follows: ， ， ， ， ， in N represents the number of basis functions in the nonlinear term, Indicates the j basis functions, is the weight corresponding to the basis function, for i The phase corresponding to the moment, is the total duration of the demonstration trajectory, represents the center of the Gaussian basis function, represents the width of the Gaussian basis function, α is the speed adjustment coefficient; Construct the following matrix: ， ， ， in is the basis function matrix, is the nonlinear term matrix, As the weight matrix, we get the following formula: ， In the above formula, the basis function matrix Directly determined by prior parameter definition, the nonlinear term matrix Calculation is performed using demonstration trajectory data, and the calculation formula is as follows: x The formula for the directional nonlinear term is as follows: ， in 、 are the second-order system parameters, for i Demonstration trajectory of the moment x coordinate, The origin of the target coordinate system x coordinate; y The formula for the directional nonlinear term is as follows: ， in 、 are the second-order system parameters, for i Demonstration trajectory of the moment y coordinate, The origin of the target coordinate system y coordinate; z The formula for the directional nonlinear term is as follows: ， ， ， in 、 are the second-order system parameters, for i Demonstration trajectory of the moment z coordinate, Demonstrate trajectory for the initial moment z coordinate, The origin of the target coordinate system z coordinate, for i Moment control coefficient; In getting F After the matrix, the weight matrix is ​​further obtained by pseudo-inverse method W Approximate solution: 。 4. The real-time skill trajectory generation method according to claim 2, characterized in that: In step 3, the origin position of the task object coordinate system at the next moment is generated by calculating x 、 y, z The coordinates of the three directions are obtained. x The calculation expression of the value of the direction at the next moment is: ， ， ， ， ， ， In the above formula, Is the new target object coordinate system origin x coordinate, Is the new starting object coordinate system origin x coordinate, The targets in the demonstration trajectory are x Coordinates and starting point x coordinate, It is a manually selected moment, and the subsequent trajectory will be close to the demonstration trajectory. is the generated control parameter, is its derivative with respect to time, The origin of the current object coordinate system x coordinate, is its derivative with respect to time, is the second-order derivative with respect to time; y To calculate the value of the next moment in the direction, the above x Parameters replaced with y The parameters corresponding to the direction are sufficient; exist z The calculation expression of the value of the direction at the next moment is: ， ， ， ， ， ， in for i The new structure of time z Coordinates and the corresponding first-order and second-order time derivatives, for i +1 The origin of the object coordinate system actually generated at time z coordinate.

5. The real-time skill trajectory generation method according to claim 4, characterized in that: The step 4 specifically includes: Calculate the first i The closest distance between the object and the obstacle at time +1 , Indicates the i Object coordinate system information at time +1: ; Collision detection is judged as When the obstacle avoidance gradient is calculated, the i +1 object coordinate system information To adjust the trajectory point position, the update i +1 object coordinate system information The expression is as follows: ， in step To compensate, is the obstacle influence range, To calculate the obstacle avoidance gradient; After the update is completed, recalculate the closest distance between the current object and the obstacle ; Collision detection is judged as If , it ends and returns to step 3, otherwise it recalculates the obstacle avoidance gradient according to the above formula and continues to update .

6. The real-time skill trajectory generation method according to claim 5, characterized in that: The obstacle avoidance gradient is calculated by the artificial potential field method, and the expression is as follows: ， ， ， ， ， in is the target attraction coefficient, is the modulus of the difference of two vectors, is the origin coordinate of the target coordinate system, Attract the potential field for the target, is the obstacle potential field coefficient, To avoid the obstacle potential field, for Relative to x , y , z The gradient of the direction, for Relative to x , y , z The gradient of the direction, for Relative to x , y , z Directional gradient.

Images