Trajectory planning method, trajectory planning device and computer readable storage medium

Abstract

The present application discloses a trajectory planning method, a trajectory planning device and a computer-readable storage medium, wherein the trajectory planning method comprises: planning a first interpolation trajectory of a robot arm moving from a key point to a key point and a second interpolation trajectory of a robot arm moving from a key point to a key point; obtaining a target time interval; generating a transition trajectory; and generating a target trajectory. The method of the present application can improve the trajectory smoothness of the robot arm during movement.

Description

Technical Field

[0001] The present application relates to the field of robotic arm technology, and in particular to a trajectory planning method, a trajectory planning device, and a computer-readable storage medium. Background Art

[0002] In many application scenarios, in order to ensure the safe operation of the robot arm, obstacle avoidance planning is required for the robot arm. During planning, the planning algorithm is first combined with the collision detection algorithm to sequentially plan multiple safety key points. When the robot arm is at these safety key points, the robot arm will not collide with surrounding obstacles. Then, the interpolation trajectory between two adjacent key points is planned to ensure that the robot arm will not collide with surrounding obstacles when moving along the interpolation trajectory. Finally, multiple sequentially planned key points are connected by linear interpolation to form a complete collision-free trajectory.

[0003] However, the inventors of the present application have found that the smoothness of the trajectory currently planned by the above method needs to be improved. Summary of the invention

[0004] The present application provides a trajectory planning method, a trajectory planning device and a computer-readable storage medium, which can improve the trajectory smoothness of a robotic arm.

[0005] A first aspect of an embodiment of the present application provides a trajectory planning method, the method comprising:

[0006] After generating multiple key points arranged in sequence according to the planning algorithm, the robot arm is planned to move from the key points Move to key point The first interpolation trajectory of And from the key points Move to key point The second interpolation trajectory of ,in, , is a functional expression of the movement time t of the robot arm, the first interpolation trajectory The start and end times are time, At this moment, the second interpolation trajectory The start and end times are time, At time, k=1~n and k is an integer, and the total number of key points is equal to (n+2);

[0007] Get target time interval ;

[0008] Generate the transition trajectory according to the following formula: :

[0009] Formula 1:

[0010] Generate the target trajectory according to the following formula 2 :

[0011] Formula 2:

[0012] Among them, the starting time and ending time of the transition trajectory are respectively ( )time, time, About The weight function of , the weight function It is monotonically increasing in the interval [0, 1] and When the value is 0, the weight function is equal to 0, When the value is 1, the weight function is equal to 1, and in When the value is 0, the weight function The first-order derivative of is equal to 0. When the value is 1, the weight function The first derivative of is also equal to 0.

[0013] A second aspect of an embodiment of the present application provides a trajectory planning device, which includes a processor, a memory, and a communication circuit. The processor is coupled to the memory and the communication circuit, respectively. Program data is stored in the memory. The processor implements the steps in the above method by executing the program data in the memory.

[0014] A third aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program can be executed by a processor to implement the steps in the above method.

[0015] The beneficial effect is: this application sets the weight function It is monotonically increasing in the interval [0, 1] and When the value is 0, the weight function is equal to 0, When the value is 1, the weight function Equal to 1, the robot arm can be moved from the interpolation trajectory during the transition phase Smooth transition to interpolated trajectory , thus ensuring the smoothness of the trajectory of the robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For those skilled in the art, other drawings can be obtained based on these drawings without creative work, among which:

[0017] Figure 1 It is a flowchart of an implementation method of trajectory planning of the present application;

[0018] Figure 2 yes Figure 1 A schematic diagram of the process of step S120 in one embodiment;

[0019] Figure 3 yes Figure 1 A schematic flow chart of step S120 in another embodiment;

[0020] Figure 4 yes Figure 1 A schematic flow chart of step S120 in another embodiment;

[0021] Figure 5 It is a structural schematic diagram of an implementation method of a trajectory planning device of the present application;

[0022] Figure 6 It is a structural schematic diagram of an implementation method of a computer-readable storage medium of the present application. DETAILED DESCRIPTION

[0023] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0024] It should be noted that the terms "first" and "second" in this application are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of indicated technical features. Thus, the features defined as "first" and "second" can explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "multiple" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally also includes steps or units that are not listed, or optionally also includes other steps or units inherent to these processes, methods, products or devices.

[0025] See also Figure 1 In one embodiment of the present application, the trajectory planning method includes:

[0026] S110: After generating a plurality of key points arranged in sequence according to the planning algorithm, the robot arm is planned to move from the key points Move to key point The first interpolated trajectory and the key points Move to key point The second interpolation trajectory of .

[0027] The first interpolation trajectory is recorded as , and the second interpolation trajectory is recorded as .in, , is a functional expression of the motion time t of the robot arm. The first interpolation trajectory The start and end times are time, Moment, the second interpolation trajectory The start and end times are time, At this moment, k=1~n and k is an integer, and the total number of key points is equal to (n+2).

[0028] Specifically, when planning the motion trajectory of the robotic arm, a plurality of key points arranged in sequence are first planned. When the robotic arm is at the position of these key points, the robotic arm will not collide with surrounding obstacles. Then, according to the arrangement order of the key points, the linear interpolation trajectory between two adjacent key points is planned in sequence.

[0029] Among them, multiple key points are recorded in order from the starting point to the end point. , ,…… , , ,…… , where k is an integer from 1 to n, and the total number of key points is equal to (n+2). , Key Points , Key Points are any three adjacent key points among multiple key points.

[0030] After generating multiple key points according to the planning algorithm, a linear interpolation trajectory is generated between any two adjacent key points, where With key points The linear interpolation trajectory between is defined as the first interpolation trajectory, and the key points With key points The linear interpolation trajectory between is defined as the second interpolation trajectory, and the first interpolation trajectory is expressed as Indicates that the second interpolation trajectory is expressed as a function At the same time, the first interpolation trajectory The starting time and ending time of time, At this moment, the second interpolation trajectory The starting time and ending time of time, time.

[0031] At this time, the following formula can be used to express the robot arm from the key point Move to key points The initial motion trajectory :

[0032]

[0033] S120: Obtain a target time interval.

[0034] Specifically, the target time interval It can be a pre-set fixed value or determined by an algorithm. The specific determination process can be found below.

[0035] S130: Generate a transition trajectory.

[0036] Specifically, the transition trajectory is generated according to the following formula: :

[0037] Formula 1:

[0038] S140: Generate a target trajectory.

[0039] Specifically, the target trajectory is generated according to the following formula 2: .

[0040] Formula 2:

[0041] Among them, the transition trajectory The starting time and ending time of )time, time, About The weight function of , weight function It is monotonically increasing in the interval [0, 1] and When the value is 0, the weight function is equal to 0, When the value is 1, the weight function is equal to 1, and in When the value is 0, the weight function The first-order derivative of is equal to 0. When the value is 1, the weight function The first derivative of is also equal to 0.

[0042] Specifically, from Time is up ( ) moment, let the robot follow the first interpolation trajectory To exercise; ) Time has come At this moment, let the robot follow the transition trajectory to exercise; Time is up ( ) moment, let the robot follow the third interpolation trajectory Exercise. time, The moment is called the transition moment. ) Time has come The moment is called the transition phase.

[0043] Among them from ) Time has come In this transition phase, according to the above formula, the first interpolation trajectory , the third interpolation trajectory Perform weighted summation to obtain the transition trajectory , and set the weight function used in the weighted summation process In the interval [0, 1], it is monotonic. When it is equal to 0, is equal to 0, that is ,when When it is equal to 1, is equal to 1, that is .

[0044] Therefore, the transition time t = , =0, at this time = , when the transition time t= , =1, at this time = , and because the weight function It is monotonically increasing in the interval [0, 1], so from Time has come time, from Gradually increase to , so in At this moment, the position R of the robot arm satisfies:

[0045]

[0046] Therefore At this moment, the position R of the robot arm satisfies:

[0047]

[0048] From the above, we can see that the transition trajectory The setting can ensure that the robot moves from the first interpolation trajectory Smooth transition to the third interpolation trajectory , thus ensuring the smoothness of the trajectory of the robot arm.

[0049] Among them, the weight function The first-order derivative, second-order derivative, and third-order derivative of are respectively denoted as , as well as , in one embodiment, , as well as The following conditions are met:

[0050]

[0051] That is, in When the value is 0, the weight function The first-order derivative function and weight function of The second-order derivative function and weight function The third-order derivative functions of are all equal to 0. When the value is 1, the weight function The first-order derivative function and weight function of The second-order derivative function and weight function The third-order derivative of is equal to 0.

[0052] The setting of the above conditions can make the speed, acceleration and jerk of the robot arm smooth and continuous without jumps at the transition moment.

[0053] Taking speed as an example, when When , the speed of the robot arm can be determined according to the following formula :

[0054]

[0055] when When , the speed of the robot arm can be determined according to the following formula :

[0056]

[0057] when When , the speed of the robot arm can be determined according to the following formula :

[0058]

[0059] in, yes The first-order derivative of yes The first derivative function of .

[0060] From the above, it can be seen that at the moment of transition , When the robot arm speed is connected, it is smooth and continuous without jumps, as shown in the following formula:

[0061]

[0062] In another transitional moment , At this time, the robot arm speed is smooth and continuous without jump before and after connection, as shown in the following formula:

[0063]

[0064] So set It can ensure that the speed of the robot arm is smooth, continuous and without jumps at the transition moment, thereby ensuring the stability of the movement of the robot arm.

[0065] Furthermore, setting , which can ensure that the acceleration of the robot arm is smooth and continuous without jumps at the transition moment, and set , which can ensure that the acceleration of the robot arm is smooth, continuous and without jumps at the transition moment, further ensuring the smooth movement of the robot arm.

[0066] In one embodiment, in order to further ensure the smooth operation of the robot arm at the transition moment, When the value is 0, the weight function The higher-order derivatives of are all equal to 0. When the value is 1, the weight function The higher-order derivatives of are also equal to 0.

[0067] In one embodiment, the weight function The expression is as follows:

[0068] .

[0069] In this embodiment, the weight function Satisfaction: In When the value is 0, the weight function The first-order derivative, second-order derivative, third-order derivative, and fourth-order derivative of are all equal to 0. When the value is 1, the weight function The first, second, third, and fourth derivatives of are all equal to 0.

[0070] See also Figure 2 In one embodiment, step S120 includes:

[0071] S121: Set the target time interval The current value of is set as the initial value.

[0072] Specifically, , where the initial value Can be a preset smaller default initial value.

[0073] S122: Generate a transition trajectory.

[0074] Specifically, Substitute into the above formula 1 to generate the transition trajectory.

[0075] S123: Determine whether the robot arm will collide with surrounding obstacles when moving along the transition trajectory.

[0076] The transition trajectory is detected using a collision detection algorithm to determine whether the robot arm will collide with surrounding obstacles when moving along the transition trajectory. If no collision occurs, step S124 is executed; if a collision occurs, step S125 is executed.

[0077] Specifically, the target time interval The smaller the value, the smaller the transition stage, the smaller the deviation of the transition trajectory from the original planned trajectory, and the smaller the probability of the robot arm colliding with the surrounding when moving along the transition trajectory. Therefore, in general, when the target time interval is set to After the current value of is set as the initial value to generate the transition trajectory, the robot arm will not collide with the surrounding area when moving along the transition trajectory.

[0078] S124: Increase target time interval The current value of .

[0079] After executing step S124, return to execute step S122.

[0080] The target time interval can be The current value of the target time interval can be increased by a preset fixed value. The current value of can be increased by a fixed multiple, or the target time interval can be increased by the principle of dichotomy. In summary, this application aims to increase the target time interval There is no restriction on the specific process of obtaining the current value of .

[0081] S125: Determine the value of the target time interval corresponding to the transition trajectory when it is most recently determined that no collision will occur as the final value of the target time interval.

[0082] Specifically, if step S123 determines that the robot arm will collide when moving along the transition trajectory, it means that the target time interval If the current value of is too large, the value of the target time interval corresponding to the transition trajectory when it is determined that no collision will occur is directly determined as the final value of the target time interval. However, if step S123 determines that the robot arm will not collide when moving along the transition trajectory, it means that the target time interval If there is still room for the value of to increase, continue to increase the target time interval The value of , and then repeat the previous steps to perform the iterative process.

[0083] Through the above iterative process, the target time interval can be automatically determined The value is easy to obtain and the efficiency is high.

[0084] See also Figure 3 In another embodiment, step S120 includes:

[0085] S221: Set the target time interval The current value of is set as the initial value.

[0086] S222: Generate a transition trajectory.

[0087] S223: Determine whether the robot arm will collide with surrounding obstacles when moving along the transition trajectory.

[0088] If it is determined that no collision will occur, step S224 is executed; otherwise, step S228 is executed.

[0089] S224: Obtain the total number of times the step of increasing the current value of the target time interval has been executed before.

[0090] That is, the number of times step S227 has been executed before, that is, the number of iterations, is obtained. The number of iterations can be counted by setting a timer. For example, the initial value of the timer is set to 0, and each time step S227 is executed, the counter automatically increases by one.

[0091] S225: Determine whether the total number of times exceeds the first number threshold.

[0092] If it exceeds the first number threshold, execute step S226, otherwise, execute step S227.

[0093] S226: Determine the current value of the target time interval as the final value of the target time interval.

[0094] S227: Increase the current value of the target time interval.

[0095] After step S227, the process returns to step S222.

[0096] S228: Determine the value of the target time interval corresponding to the transition trajectory when it is most recently determined that no collision will occur as the final value of the target time interval.

[0097] Different from the above-mentioned embodiment, this embodiment increases the target time interval. Before the current value of the target time interval is taken, the current total number of iterations will be determined. If the total number of iterations has exceeded the first number threshold, the target time interval will no longer be increased. Instead of using the current value of the target time interval, the current value of the target time interval is directly determined as the final value of the target time interval. However, if it has not exceeded the first count threshold, the current value of the target time interval is increased and the previous steps are repeated. This implementation controls the number of iterations by setting the first count threshold to ensure the efficiency of the algorithm.

[0098] See also Figure 4 In another embodiment, step S120 includes:

[0099] S321: Set the target time interval The current value of is set as the initial value.

[0100] S322: Generate a transition trajectory.

[0101] S323: Determine whether the robot arm will collide with surrounding obstacles when moving along the transition trajectory.

[0102] If it is determined that no collision will occur, step S324 is executed; if it is determined that a collision will occur, step S328 is executed.

[0103] S324: Obtain the total number of times the step of increasing the current value of the target time interval has been executed before.

[0104] The process is the same as step S224.

[0105] S325: Determine whether the total number of times exceeds the first number threshold.

[0106] If it exceeds the first number threshold, execute step S326, otherwise, execute step S327.

[0107] S326: Determine the current value of the target time interval as the final value of the target time interval.

[0108] S327: Increase the current value of the target time interval.

[0109] S328: Obtain the total number of times the step of increasing the current value of the target time interval has been executed before.

[0110] The process is the same as step S224 and step S324.

[0111] S329: Determine whether the total number of times exceeds the second number threshold.

[0112] If it exceeds the second numerical threshold, execute step S330, otherwise return to execute step S327.

[0113] S330: Determine the value of the target time interval corresponding to the transition trajectory when it is most recently determined that no collision will occur as the final value of the target time interval.

[0114] Specifically, different from the above embodiment, in this embodiment, if step S232 determines that the robot arm will collide with surrounding obstacles when moving along the transition trajectory, it is determined whether the total number of iterations exceeds the second number threshold. If it exceeds, the value of the target time interval corresponding to the transition trajectory that was most recently determined not to collide is determined as the final value of the target time interval. If it does not exceed, it returns to step S327 to continue to increase the target time interval. The current value of this setting can maximize the target time interval The first number threshold and the second number threshold may be equal or different, and may be set according to actual conditions.

[0115] In the above implementations, the target time interval is The iteration process is performed starting from a smaller initial value, but in other implementations, the target time interval may also be increased. Start from a larger initial value and reduce it iteratively. Through similar methods, determine a target time interval as large as possible. . At the same time, the target time interval The larger the value, the longer the transition phase is, which allows the robot to better interpolate from the first trajectory. Smooth transition to the third interpolation trajectory , ensuring the smooth operation of the robot.

[0116] See also Figure 5 , Figure 5 1 is a schematic diagram of the structure of an embodiment of a trajectory planning device of the present application. The trajectory planning device 200 includes a processor 210, a memory 220, and a communication circuit 230. The processor 210 is coupled to the memory 220 and the communication circuit 230 respectively. The memory 220 stores program data. The processor 210 implements the steps in any of the above-mentioned implementation methods by executing the program data in the memory 220. The detailed steps can be found in the above-mentioned implementations and will not be repeated here.

[0117] The trajectory planning device 200 may be any device with algorithm processing capabilities, such as a computer, a mobile phone, a robot control cabinet, etc., and is not limited here.

[0118] See also Figure 6 , Figure 6 The computer-readable storage medium 400 stores a computer program 410, which can be executed by a processor to implement the steps in any of the above methods.

[0119] The computer-readable storage medium 400 may specifically be a device that can store the computer program 410, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, or may be a server that stores the computer program 410. The server may send the stored computer program 410 to other devices for execution, or may also execute the stored computer program 410 by itself.

[0120] The above description is only an embodiment of the present application and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly used in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A trajectory planning method, characterized in that: The method comprises: After generating multiple key points arranged in sequence according to the planning algorithm, the robot arm is planned to move from the key points Move to key point The first interpolation trajectory of And from the key points Move to key point The second interpolation trajectory of ,in, , is a functional expression of the movement time t of the robot arm, the first interpolation trajectory The start and end times are time, At this moment, the second interpolation trajectory The start and end times are time, At time, k=1~n and k is an integer, and the total number of key points is equal to (n+2); Get target time interval ; Generate the transition trajectory according to the following formula: : Formula 1: Generate the target trajectory according to the following formula 2 : Formula 2: Among them, the starting time and ending time of the transition trajectory are respectively ( )time, time, About The weight function of , the weight function It is monotonically increasing in the interval [0, 1] and When the value is 0, the weight function is equal to 0, When the value is 1, the weight function is equal to 1, and in When the value is 0, the weight function The first-order derivative of is equal to 0. When the value is 1, the weight function The first-order derivative of is also equal to 0; Wherein, the target time interval is obtained The steps include: The target time interval The current value of is set to the initial value; Generate the transition trajectory according to the formula 1; Determine whether the robot arm will collide with surrounding obstacles when moving along the transition trajectory; In response to no collision occurring, increasing the target time interval The current value of , and then returns to execute the step of generating the transition trajectory according to the formula 1; In response to a collision, the target time interval corresponding to the transition trajectory when it was most recently determined that no collision would occur is The value of is determined as the final value of the target time interval.

2. The method according to claim 1, characterized in that The weight function is high-order and can be differentiated. When the value is 0, the weight function The second-order derivative function, the weight function The third-order derivative functions of are all equal to 0. When the value is 1, the weight function The second-order derivative function, the weight function The third-order derivative of is equal to 0.

3. The method according to claim 2, characterized in that exist When the value is 0, the weight function The higher-order derivative of is equal to 0, When the value is 1, the weight function The higher-order derivatives of are also equal to 0.

4. The method according to claim 1, characterized in that: The weight function The expression is as follows: 。 5. The method according to claim 1, characterized in that In response to no collision occurring, the target time interval is increased Before the current value of , it also includes: In response to the collision not occurring, obtaining the previously executed step of increasing the target time interval The total number of steps of the current value of ; In response to the total number of times exceeding a first number threshold, the target time interval The current value of is determined as the target time interval The final value of In response to the total number of times not exceeding the first number threshold, increasing the target time interval The step of the current value.

6. The method according to claim 1, characterized in that increasing the target time interval The steps for obtaining the current value of include: The target time interval The current value of is increased by a preset value.

7. The method according to claim 1, characterized in that increasing the target time interval The steps for obtaining the current value of include: The target time interval The current value of is enlarged by a preset multiple.

8. A trajectory planning device, characterized in that: The trajectory planning device includes a processor, a memory and a communication circuit, the processor is coupled to the memory and the communication circuit respectively, the memory stores program data, and the processor implements the steps in the method as described in any one of claims 1-7 by executing the program data in the memory.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program can be executed by a processor to implement the steps in the method according to any one of claims 1 to 7.

Images