A robot motion planning method and device, a robot, and a storage medium

Abstract

The application relates to a robot motion planning method, device, robot and storage medium, the method comprises the following steps: acquiring the generalized arc length of a path point of a robot joint and a function of the path about the generalized arc length; acquiring the generalized speed interval of each path point of the robot joint according to the generalized arc length of the path point and the function of the path about the generalized arc length, acquiring the generalized speed and the generalized acceleration of the path point according to the generalized speed interval; judging whether there is an acceleration discontinuous point in the path point of the robot joint according to the generalized acceleration, if there is, performing generalized speed interpolation near the acceleration discontinuous point; determining the function of the path of the robot joint about time according to the generalized arc length of each path point of the robot joint and the generalized speed after interpolation. The method provided by the application realizes the motion planning of a time-optimal robot, and guarantees that the acceleration of the robot joint is continuous and bounded.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot motion control, in particular to a robot motion planning method and device, a robot and a storage medium. BACKGROUND

[0002] Motion planning is one of the most important links of industrial robots, and almost any application of industrial robots cannot be separated from it. Unlike the motion planning of machine tools, the motion planning of robots has strong nonlinearity, especially near the singular point of the robot, the motion that meets the motion requirements in the Cartesian space is difficult to achieve in the joint space, so the motion planning algorithm on the machine tool cannot be directly applied to the industrial robot; the prior art proposes a time-optimal robot motion planning method for this problem, but this algorithm cannot guarantee continuous and bounded acceleration, and has no practical value in actual production process. SUMMARY

[0003] Therefore, the present application provides a robot motion planning method, comprising the following steps:

[0004] Obtain the generalized arc length of the path point of the robot joint and the function of the path with respect to the generalized arc length;

[0005] According to the generalized arc length of the path point and the function of the path with respect to the generalized arc length, obtain the generalized velocity interval of each path point of the robot joint, and obtain the generalized velocity and generalized acceleration of the path point according to the generalized velocity interval;

[0006] According to the generalized acceleration, determine whether there is an acceleration discontinuous point in the path point of the robot joint, if there is, perform generalized velocity interpolation near the acceleration discontinuous point;

[0007] According to the generalized arc length of each path point of the robot joint and the interpolated generalized velocity, determine the function of the path of the robot joint with respect to time.

[0008] Optionally, according to the generalized arc length of the path point and the function of the path with respect to the generalized arc length, the generalized velocity interval at each path point of the robot joint is obtained, comprising:

[0009] According to the generalized arc length of each path point of the robot joint and the function of the path with respect to the generalized arc length, determine the constraint condition;

[0010] Linear programming is performed with generalized velocity and generalized acceleration as variables to obtain the maximum generalized velocity and the minimum generalized velocity allowed for the current path point, and the interval formed by the minimum generalized velocity and the maximum generalized velocity is taken as the generalized velocity interval of the current path point.

[0011] Alternatively, when the maximum generalized velocity allowed at the current path point is obtained, the corresponding linear programming is described as

[0012] max x

[0013]

[0014] min u i+1 ≤u≤u max

[0015]

[0016]

[0017] 0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2

[0018] min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1

[0019] wherein x is the square of the generalized velocity, u is the generalized acceleration, is the upper limit of the generalized velocity, u max represents the upper limit of the generalized acceleration u, min u i is the lower limit of the generalized acceleration at the current path point, subscript i is the serial number of the current path point, p’(l) and p”(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 is the square of the maximum generalized velocity allowed at the i+1th path point, v max is the maximum linear velocity of the robot end in the world coordinate system, j is the serial number of the robot axis, and J is the Jacobian matrix.

[0020] Alternatively, when the minimum generalized velocity allowed at the current path point is obtained, the corresponding linear programming is described as

[0021] min x

[0022]

[0023] min ui+1 ≤u≤u max

[0024]

[0025]

[0026] 0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2

[0027] min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1

[0028] where x is the square of generalized velocity, u is the generalized acceleration, is the upper limit of generalized velocity, u max represents the upper limit of generalized acceleration u, min u i is the lower limit of generalized acceleration at the current path point, subscript i is the serial number of the current path point, p'(l) and p''(l) are the first derivative and second derivative of the path with respect to the generalized arc length function respectively, max x i+1 is the square of the maximum generalized velocity allowed for the i+1th path point, v max is the maximum linear velocity of the robot end in the world coordinate system, j is the serial number of the robot axis, and J is the Jacobian matrix.

[0029] Optionally, the robot motion planning method further comprises, when determining the acceleration lower limit of the path point, if the maximum generalized velocity allowed for the current path point is greater than the maximum generalized velocity corresponding to the previous path point, updating the acceleration lower limit of the next path point according to the maximum generalized velocity allowed for the current path point.

[0030] Optionally, the generalized velocity and the generalized acceleration of the path point are obtained according to the generalized velocity interval, comprising:

[0031] determining a constraint condition according to the generalized velocity interval, performing linear programming with the generalized velocity and the generalized acceleration as variables to obtain the generalized velocity and the generalized acceleration of each path point.

[0032] Optionally, the linear programming is described as

[0033] max u

[0034]

[0035] min u i ≤u≤ max u i

[0036]

[0037]

[0038] 0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2

[0039] min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1

[0040] where x is the square of generalized velocity, u is the generalized acceleration, is the upper limit of generalized velocity, u max represents the upper limit of generalized acceleration u, min u i is the lower limit of generalized acceleration at the current path point, subscript i is the serial number of the current path point, p'(l) and p''(l) are the first derivative and second derivative of the path with respect to the generalized arc length function respectively, max x i+1 is the square of the maximum generalized velocity allowed for the i+1th path point, v max is the maximum linear velocity of the robot end in the world coordinate system, is the generalized velocity at the current path point, max u i is the upper limit of generalized acceleration at the current path point, j is the serial number of the robot axis, and J is the Jacobian matrix.

[0041] Optionally, according to the generalized acceleration, it is judged whether there is a point of acceleration discontinuity in the robot joint path point, comprising: if the absolute value of the difference between the generalized acceleration of the current path point and the previous path point is greater than a preset threshold, it is determined that the current path point is a point of acceleration discontinuity.

[0042] Optionally, the path of the robot joint as a function of time is determined according to the generalized arc length and the interpolated generalized velocity of each path point of the robot joint, comprising:

[0043] The time sequence corresponding to the robot joint is obtained according to the generalized arc length and the interpolated generalized velocity of each path point of the robot joint.

[0044] The path as a function of time is obtained by fitting the generalized arc length of the path point of the robot joint and the time sequence.

[0045] The application further provides a robot motion planning device, comprising a data processing module, a linear programming module, an interpolation module and a path planning module.

[0046] The data processing module is configured to obtain the generalized arc length of the path point of the robot joint and the function of the path with respect to the generalized arc length.

[0047] The linear programming module is configured to obtain the generalized velocity interval of each path point of the robot joint according to the generalized arc length of the path point and the function of the path with respect to the generalized arc length, and obtain the generalized velocity and the generalized acceleration of the path point according to the generalized velocity interval.

[0048] The interpolation module is configured to determine whether there is a point of discontinuous acceleration in the path point of the robot joint according to the generalized acceleration, and if there is, to interpolate the generalized velocity near the point of discontinuous acceleration.

[0049] The path planning module is configured to determine the path of the robot joint as a function of time according to the generalized arc length and the interpolated generalized velocity of each path point of the robot joint.

[0050] The application further provides a robot, comprising a processor and a memory coupled to the processor, wherein the memory stores program instructions executable by the processor; and the processor executes the program instructions stored in the memory to implement the robot motion planning method according to any of the above technical solutions.

[0051] The application further provides a storage medium, wherein the storage medium stores program instructions executable by a processor to implement the robot motion planning method according to any of the above technical solutions.

[0052] The robot motion planning method, device, robot and storage medium provided by the application, by acquiring the generalized arc length of the robot joint path point and the function of the path about the generalized arc length, further acquiring the generalized speed interval at the robot joint path point, acquiring the generalized speed and generalized acceleration at the robot joint path point according to the generalized speed interval, performing generalized speed interpolation near the acceleration discontinuous point, and determining the function of the path about time according to the generalized arc length of the robot joint path point and the interpolated generalized speed, the motion planning of the robot with optimal time is realized, the robot joint acceleration is ensured to be continuous and bounded, and the robot joint motion trajectory is smooth. BRIEF DESCRIPTION OF DRAWINGS

[0053] Figure 1 The flowchart of the robot motion planning method provided by the embodiment of the application;

[0054] Figure 2 The two-dimensional graph of the path about the generalized arc length provided by the embodiment of the application;

[0055] Figure 3 The two-dimensional graph of the first derivative about the generalized arc length at the path point column provided by the embodiment of the application;

[0056] Figure 4 The two-dimensional graph of the second derivative about the generalized arc length at the path point column provided by the embodiment of the application;

[0057] Figure 5 The two-dimensional graph of the allowed speed interval about the generalized arc length at the path point column provided by the embodiment of the application;

[0058] Figure 6 The two-dimensional graph of the square of the generalized acceleration and generalized speed about the generalized arc length at the path point column provided by the embodiment of the application;

[0059] Figure 7 The curve graph of the generalized speed about the generalized arc length provided by the embodiment of the application;

[0060] Figure 8 The two-dimensional graph of the time sequence about the generalized arc length provided by the embodiment of the application;

[0061] Figure 9 The two-dimensional graph of the path about time provided by the embodiment of the application;

[0062] Figure 10 The structural schematic diagram of the robot motion planning device provided by the embodiment of the application;

[0063] Figure 11 The structural schematic diagram of the storage medium provided by the embodiment of the application. DETAILED DESCRIPTION

[0064] For the purposes of promoting an understanding of the principles of the application, reference will now be made to the embodiments illustrated in the drawings. It is expressly understood that the drawings are only meant to be illustrative and that the application can take many different forms. In the drawings:

[0065] It is to be understood that the singular forms "a," "an," and "the" include plural referents unless the context clearly dictates otherwise. It is to be understood that the terms "approximately" and "substantially" are used herein to represent the insubstantial difference in the precision of a numerical value. It is to be understood that when an element is referred to as being "on" another element, it can be directly on the other element or intervening elements can also be present. In addition, it is to be understood that when an element is referred to as being "connected" to another element, it can be directly connected to the other element or intervening elements can also be present.

[0066] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used in the description of the application herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application.

[0067] Figure 1 is a flowchart of a robot motion planning method according to a first embodiment of the present application. It is to be noted that the method of the present application is not limited to the order of the flowchart shown. Figure 1 The order of the flowchart shown is not limiting.

[0068] As shown in Figure 1 , the robot motion planning method mainly includes the following steps S101-S104:

[0069] S101, obtaining the generalized arc length of the path point of the robot joint and the function of the path about the generalized arc length;

[0070] Wherein, the robot joint path point forms a robot joint path point list, the arc length corresponding to the path point can be obtained according to the robot joint path point list, and the calculation formula of the arc length corresponding to the path point is l i+1 = l i + |p i+1 -p i |2, l i is the arc length corresponding to the path point p i , l i+1 is the arc length corresponding to the path point p i+1 , since the path points are very close, the chord length is used instead of the arc length here, and after the arc length corresponding to the path point list is calculated, the function p(l) of the path about the generalized arc length can be obtained by using the cubic spline curve fitting.

[0071] It should be noted that the motion planning of the embodiment of the present application is planning in joint space (joint coordinate system), and for a robot with six joints, a six-dimensional path about the function of generalized arc length can be obtained.

[0072] In one specific embodiment, the robot kinematics parameters are shown in Table 1.

[0073] Table 1: Robot kinematics parameters

[0074]

[0075] In Table 1, link number is joint number, a is the distance of two joint axes along the common axis, α is the included angle between the two joint axes, d is the joint translation distance, and θ is the joint translation angle. The maximum speed of each joint of the robot is π / s, and the maximum acceleration of each joint is 10π / s 2 The path point list is fitted by a cubic spline to obtain a two-dimensional graph of the path about the generalized arc length, as shown in Figure 2 After obtaining the function of the path point list about the generalized arc length, the first derivative and the second derivative of the function at the path point list are calculated, and the two-dimensional graphs of the first derivative and the second derivative of the path point list about the generalized arc length are obtained, as shown in Figure 3 、 Figure 4 , Figure 3 、 Figure 4 In the above equations, q1-q6 represent six different joints.

[0076] S102, according to the generalized arc length of the path point and the function of the path about the generalized arc length, obtaining the generalized velocity interval of each path point of the robot joint, and according to the generalized velocity interval, obtaining the generalized velocity and the generalized acceleration of the path point;

[0077] It should be noted that for each joint of the robot, its generalized velocity cannot exceed its generalized velocity interval, and according to the constraint condition of the function of the path about the generalized arc length, reverse iterative calculation is performed, i.e. iteration from back to front in time sequence, so that the allowed generalized velocity interval at the path point list can be obtained.

[0078] S103, according to the generalized acceleration, judging whether there is an acceleration discontinuous point in the path point of the robot joint, if there is, performing generalized velocity interpolation near the acceleration discontinuous point;

[0079] If there is no acceleration discontinuous point, interpolation operation is not needed, and generalized velocity interpolation near the acceleration discontinuous point means inserting a number of points (for example, 20 points) on the left and right of the acceleration discontinuous point, and each point is separated from the previous point by a preset arc length (for example, 0.02 rad).

[0080] S104, determine the function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the generalized velocity after interpolation.

[0081] The robot motion planning method provided by the embodiment of the application realizes the time-optimal robot motion planning, ensures the continuity and boundedness of the acceleration of the robot joint, ensures that the robot joint can smoothly pass through the robot singularity point, and makes the robot joint motion trajectory smooth.

[0082] In an optional embodiment, the generalized velocity interval at each path point of the robot joint is obtained according to the generalized arc length of the path point and the function of the path with respect to the generalized arc length, and the method comprises:

[0083] The constraint condition is determined according to the generalized arc length of each path point of the robot joint and the function of the path with respect to the generalized arc length.

[0084] The linear programming is performed with the generalized velocity and the generalized acceleration as variables to obtain the maximum generalized velocity and the minimum generalized velocity allowed at the current path point, and the interval formed by the minimum generalized velocity and the maximum generalized velocity is taken as the generalized velocity interval of the current path point.

[0085] The maximum generalized velocity and the minimum generalized velocity at the path point column can be obtained by performing the reverse iterative calculation according to the constraint condition of the function of the path with respect to the generalized arc length, so that the generalized velocity interval allowed at the path point column can be determined.

[0086] In an optional embodiment, when the maximum generalized velocity allowed at the current path point is obtained, the linear programming is described as

[0087] max x

[0088]

[0089] min u i+1 ≤u≤u max

[0090]

[0091]

[0092] 0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2

[0093] min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1

[0094] wherein x is the square of the generalized velocity, u is the generalized acceleration, is the upper limit of the generalized velocity, u max represents the upper limit of the generalized acceleration u, min u i is the lower limit of the generalized acceleration at the current path point, subscript i is the serial number of the current path point, p’(l) and p”(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 is the square of the maximum generalized velocity allowed for the i+1th path point, v max is the maximum linear velocity of the robot end in the world coordinate system, j is the serial number of the robot axis, and J is the Jacobian matrix.

[0095] The above linear programming is a second-order linear programming, and the variables are x and u respectively; the lower limit of the generalized acceleration is obtained by back iteration, so it changes with the back iteration process, while the upper limit of the generalized acceleration u max is a constant, the third row constraint in the constraint condition represents the velocity constraint of each axis of the robot, the fourth row constraint in the constraint condition is the acceleration constraint of each axis (joint) of the robot, and the fifth row in the constraint condition is the linear velocity constraint of the robot end in the world coordinate system; the sixth constraint in the constraint condition establishes the constraint between the two path points, i.e. uniform acceleration motion is performed between the two path points; through the solution of the above linear programming, the square of the maximum generalized velocity allowed for the current path point max x i can be obtained.

[0096] In one embodiment, consider the 500th path point, q500 = [-0.0000 -0.2640 0.2358 0.0000 0.0283 -0.0000], the first derivative with respect to generalized arc length at this point is q500' = [0.0000 0.7584 -0.6298 0.0000 -0.1286 -0.0000], and the second derivative is q500" = [-0.0000 -1.1484 -1.7885 -0.0000 2.9369 0.0000], min x 501 is 0, is 10, u max is 50, v max is 0.25, max x 501 is 1.3871, min u 501 is -0.06; thus solving the linear programming problem for x max x 500 can be expressed as

[0097] max x

[0098] s.t. 0 < x < 100

[0099] -0.06 < u < 50

[0100] 0 < 0 < x < π 2

[0101] 0 < 0.5751 < x < π 2

[0102] 0 < 0.3966 < x < π 2

[0103] 0 < 0 < x < π 2

[0104] 0 < 0.0165 < x < π 2

[0105] 0 < 0 < x < π 2

[0106] -10π < 0 < x + 0 < u < 10π

[0107] -10π < -1.1484 < x + 0.7584 < u < 10π

[0108] -10π < -1.7885 < x - 0.6298 < u < 10π

[0109] -10π < 0 < x + 0 < u < 10π

[0110] -10π < 2.9369x - 0.1286u < 10π

[0111] -10π < 0x + 0u < 10π

[0112] 0 < 0.0322x < 0.0625

[0113] 0 < x + 0.004u < 1.3871

[0114] Solving for x max x 500 is 1.3752.

[0115] In an alternative embodiment, when the minimum generalized speed allowed at the current path point is obtained, the corresponding linear programming is described as

[0116] min x

[0117]

[0118] min u i+1 < u < u max

[0119]

[0120]

[0121] 0 < p'(l i ) T J(p(l i )) T J(p(l i )) p'(l i ) x < v max 2

[0122] min x i+1 < x + 2(l i+1 - l i ) u < x max x i+1

[0123] where x is the square of the generalized speed, u is the generalized acceleration, is the upper limit of the generalized speed, u max denotes the upper limit of the generalized acceleration u, min u i is the lower limit of the generalized acceleration at the current path point, subscript i is the serial number of the current path point, p'(l), p"(l) are the first and second derivatives of the path with respect to the generalized arc length function, max x i+1v is the square of the maximum allowed generalized velocity at the (i+1)th path point. max Let be the maximum linear velocity of the robot's end effector in the world coordinate system, j be the robot axis number, and J be the Jacobian matrix.

[0124] It should be noted that the linear programming problem described above is also a second-order linear programming problem, with the variables being x and u respectively. By solving the linear programming problem above, we can obtain the square of the minimum generalized velocity allowed by the current path point. min x i .

[0125] In one specific embodiment, min x 500 This can be obtained by solving the following linear problem.

[0126] min x

[0127] st0≤x≤100

[0128] -0.06≤u≤50

[0129] 0 < 0x < π 2

[0130] 0 < 0.5751x < π 2

[0131] 0 < 0.3966x < π 2

[0132] 0 < 0x < π 2

[0133] 0 < 0.0165x < π 2

[0134] 0 < 0x < π 2

[0135] -10π<0x+0u<10π

[0136] -10π < -1.1484x + 0.7584u < 10π

[0137] -10π < -1.7885x - 0.6298u < 10π

[0138] -10π<0x+0u<10π

[0139] -10π < 2.9369x - 0.1286u < 10π

[0140] -10π<0x+0u<10π

[0141] 0 ≤ 0.0322x ≤ 0.0625

[0142] 0 < x + 0.004u < 1.3871

[0143] Solving min x 500 = 0, since max x 500 max x 501 = 0, since min u 500 and min u 501 take the same value, i.e. min u 501 = -0.06, the two-dimensional plot of the allowed speed interval on the generalized arc length for the path point sequence is shown in Fig. 2. Figure 5

[0144] In an alternative embodiment, the robot motion planning method further comprises, when determining the acceleration lower bound of a path point, if the maximum generalized speed allowed by the current path point is greater than the maximum generalized speed allowed by the previous path point, updating the acceleration lower bound of the next path point according to the maximum generalized speed allowed by the current path point.

[0145] wherein, when determining the generalized acceleration lower bound, if the maximum generalized speed allowed by the current step (current path point) is greater than the maximum generalized speed allowed by the previous iteration (previous path point), the generalized acceleration lower bound of the next iteration (next path point) is updated according to the maximum generalized speed allowed by the current step, otherwise, the generalized acceleration lower bound of the next iteration is the same as the generalized acceleration of the current step.

[0146] When the maximum generalized speed allowed by the current step is greater than the maximum generalized speed allowed by the previous iteration, the calculation formula of the acceleration lower bound of the next iteration is min u i = min u i+1 - k · (l i+1 - l i ) / sqrt( max x i ), where k is a pre-set generalized jerk upper limit. This step actually adds a constraint to the above-mentioned second-order linear programming to ensure that the generalized acceleration of the deceleration segment is continuous and the generalized jerk is bounded.

[0147] In an alternative embodiment, the generalized speed and generalized acceleration of the path point are obtained according to the generalized speed interval, comprising:

[0148] determining a constraint condition according to the generalized speed interval, and performing linear programming with the generalized speed and generalized acceleration as variables to obtain the generalized speed and generalized acceleration of each path point. ​​

[0149] It should be noted that the linear programming is performed with the generalized velocity and the generalized acceleration as variables to obtain the generalized velocity and the generalized acceleration of all path points, and the iteration process is a forward iteration, i.e., iteration from the front to the back in time sequence.

[0150] In an optional embodiment, the linear programming is described as

[0151] max u

[0152]

[0153] min u i ≤u≤ max u i

[0154]

[0155]

[0156] 0≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x≤v max 2

[0157] min x i+1 ≤x+2(l i+1 -l i )u≤ max x i+1

[0158] wherein x is the square of the generalized velocity, u is the generalized acceleration, is the upper limit of the generalized velocity, u max represents the upper limit of the generalized acceleration u, min u i is the lower limit of the generalized acceleration at the current path point, the subscript i is the serial number of the current path point, p’(l) and p”(l) are the first derivative and the second derivative of the path with respect to the generalized arc length function respectively, max x i+1 is the square of the maximum generalized velocity allowed for the i+1th path point, v max is the maximum linear velocity of the robot end in the world coordinate system, is the generalized velocity at the current path point, max u i is the upper limit of the generalized acceleration at the current path point, j is the serial number of the robot axis, and J is the Jacobian matrix.

[0159] It should be noted that, is the generalized velocity at the current path point, max u i is the upper limit that changes with the iteration process; solving the linear programming problem can obtain the generalized acceleration u i at the current path point, the generalized velocity u at the next path point can be calculated by the following formula:

[0160]

[0161] The acceleration upper limit of the next path point can be calculated by the following formula:

[0162]

[0163] This step is actually equivalent to adding a constraint in the above linear rule, so that the generalized acceleration of the acceleration segment is continuous, and the generalized acceleration is bounded.

[0164] In one specific embodiment, again taking the 500th path point as an example, at this time, there is the following linear programming problem:

[0165] maxx

[0166] s.t.1.3191≤x≤1.3191

[0167] -0.06≤u≤2.2957

[0168] 0<0x<π 2

[0169] 0<0.5751x<π 2

[0170] 0<0.3966x<π 2

[0171] 0<0x<π 2

[0172] 0<0.0165x<π 2

[0173] 0<0x<π 2

[0174] -10π<0x+0u<10π

[0175] -10π<-1.1484x+0.7584u<10π

[0176] -10π<-1.7885x-0.6298u<10π

[0177] -10π < 0x + 0u < 10π

[0178] -10π < 2.9369x - 0.1286u < 10π

[0179] -10π < 0x + 0u < 10π

[0180] 0 < 0.0322x < 0.0625

[0181] 0 < x + 0.004u < 1.3871

[0182] Solving u 500 is 2.2838, so the next iteration of x 501 = 1.3191 + 2*0.002*2.2838 = 1.3282; the upper bound of the generalized acceleration of the next iteration can be calculated by

[0183] max u 501 = 2.2838 + 50*0.002 / √1.3282 = 2.3706

[0184] The two-dimensional graph of the square of the generalized acceleration and the generalized velocity on the path point column with respect to the generalized arc length is shown in FIG. 1. Figure 6

[0185] In an optional embodiment, according to the generalized acceleration, it is determined whether there is a point of acceleration discontinuity in the robot joint path point, including: if the absolute value of the difference between the generalized acceleration of the current path point and the previous path point is greater than a preset threshold, it is determined that the current path point is a point of acceleration discontinuity.

[0186] where the inequality |u i+1 -u i |>Δu max is used to determine whether there is a point of acceleration discontinuity in the robot joint path point, if the inequality is satisfied, it is determined that there is a point of acceleration discontinuity in the robot joint path point, and interpolation is needed near the i+1th path point. Specifically, a cubic Bezier curve can be used to interpolate the generalized velocity near the point of acceleration discontinuity to ensure the continuity of the generalized acceleration and the boundedness of the acceleration.

[0187] In a specific embodiment, the point of acceleration discontinuity is determined according to the calculated acceleration, and Δu is set to 3, from Figure 6 ​As can be seen, there are four points where the acceleration is discontinuous, located near the generalized arc lengths of 0.06, 0.9, 1.8, and 2.5, respectively, with corresponding path point numbers of 88, 528, 968, and 1308. Interpolating the generalized velocity near these discontinuities using cubic Bézier curves ensures the continuity of the generalized acceleration and that the jerk is bounded. The generalized velocity versus generalized arc length curve is shown below. Figure 7 As shown, Figure 7 The solid line represents the curve of generalized velocity with respect to generalized arc length before interpolation, while the dashed line represents the curve of generalized velocity with respect to generalized arc length after interpolation.

[0188] In an optional implementation, the path of the robot joint as a function of time is determined based on the generalized arc length of each path point of the robot joint and the interpolated generalized velocity, including:

[0189] The time series corresponding to the robot joint is obtained based on the generalized arc length of each path point of the robot joint and the generalized velocity after interpolation.

[0190] By fitting the generalized arc length of the path points of the robot joints and the time series, a function of the path with respect to time is obtained.

[0191] Specifically, the time series corresponding to the robot joint path points is obtained based on the generalized arc length of the robot joint path points and the interpolated generalized velocity. The specific calculation formula is as follows:

[0192]

[0193] By fitting the generalized arc length of the robot joint path points and the time series using a cubic spline curve, a path function with respect to time can be obtained.

[0194] In one specific embodiment, a two-dimensional graph of the time series with respect to the generalized arc length, such as Figure 8 As shown, the path function p(t) with respect to time is obtained by fitting a cubic spline curve. The two-dimensional graph of the path with respect to time is shown below. Figure 9 As shown, Figure 9 In the diagram, q1-q6 represent six different joints.

[0195] A schematic diagram of the robot motion planning device according to the second embodiment of the present invention is shown below. Figure 10As shown, the robot motion planning device 100 comprises a data processing module 101, a linear programming module 102, an interpolation module 103 and a path planning module 104; the data processing module 101 is used for acquiring the generalized arc length of a path point of a robot joint and a function of the path about the generalized arc length; the linear programming module 102 is used for acquiring the generalized velocity interval of each path point of the robot joint according to the generalized arc length of the path point and the function of the path about the generalized arc length, and acquiring the generalized velocity and the generalized acceleration of the path point according to the generalized velocity interval; the interpolation module 103 is used for judging whether there is a point of acceleration discontinuity in the path point of the robot joint according to the generalized acceleration, and if there is, performing generalized velocity interpolation near the point of acceleration discontinuity; and the path planning module 104 is used for determining the function of the path of the robot joint about time according to the generalized arc length of each path point of the robot joint and the generalized velocity after interpolation.

[0196] The third embodiment of the present application provides a robot, comprising a processor and a memory coupled with the processor, the memory storing program instructions executable by the processor; the processor executes the program instructions stored in the memory to implement the robot motion planning method of any of the above embodiments.

[0197] Figure 11 The fourth embodiment of the present application is a storage medium, the storage medium 110 stores program instructions 111, the program instructions 111 are executed by the processor to implement the robot motion planning method of any of the above embodiments. The storage medium can be non-volatile or volatile. The program instructions 111 can be stored in the above storage medium in the form of a software product, and the storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk or an optical disk, and various media that can store program codes.

[0198] The technical features of the above embodiments can be combined arbitrarily, in order to make the description concise, not all possible combinations of the technical features in the above embodiments are described, however, as long as the combination of the technical features does not exist contradictory, it should be considered as the scope of the present application.

[0199] The above examples only express the preferred embodiments of the present application, which are described in a more specific and detailed manner, but should not be understood as a limitation to the scope of the patent. It should be noted that for those skilled in the art, several modifications and improvements can be made without departing from the concept of the present application, which are all within the scope of protection of the present application. Therefore, the scope of protection of the present application patent should be subject to the appended claims.

Claims

1. A robot motion planning method, characterized by, The method comprises the steps of: obtaining a generalized arc length of a path point of a robot joint and a function of the path with respect to the generalized arc length; obtaining a generalized velocity interval of each path point of the robot joint according to the generalized arc length of the path point and the function of the path with respect to the generalized arc length, wherein the obtaining of the generalized velocity interval of each path point of the robot joint according to the generalized arc length of the path point and the function of the path with respect to the generalized arc length comprises: determining a constraint condition according to the generalized arc length of each path point of the robot joint and the function of the path with respect to the generalized arc length, performing linear programming with generalized velocity and generalized acceleration as variables, obtaining a maximum generalized velocity and a minimum generalized velocity allowed by a current path point, and taking an interval formed by the minimum generalized velocity and the maximum generalized velocity as the generalized velocity interval of the current path point; obtaining a generalized velocity and a generalized acceleration of the path point according to the generalized velocity interval, wherein the obtaining of the generalized velocity and the generalized acceleration of the path point according to the generalized velocity interval comprises: determining a constraint condition according to the generalized velocity interval, performing linear programming with generalized velocity and generalized acceleration as variables, and obtaining a generalized velocity and a generalized acceleration of each path point; judging whether there is an acceleration discontinuous point in the path point of the robot joint according to the generalized acceleration, and performing generalized velocity interpolation in the vicinity of the acceleration discontinuous point if there is the acceleration discontinuous point; determining a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the interpolated generalized velocity.

2. The robot motion planning method of claim 1, wherein, When the maximum generalized velocity allowed by the current path point is obtained, the linear programming is described as max x s.t. 0 ≤ x ≤ ṡ max 2 min u i+1 ≤ u ≤ u max 0 ≤ j p’ ( l i ) 2 x≤ j ṗ max 2 0 ≤ j p’’ ( l i ) x+ j p’(l i )u≤ j ṗ max 0 ≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x ≤ v max 2 min x i+1 ≤ x+2(l i+1 −l i )u ≤ max x i+1 wherein, x is the square of the generalized velocity, u is the generalized acceleration, ṡ max is the upper bound of the generalized velocity, u max denotes the upper bound of the generalized acceleration u , min u i is the lower bound of the generalized acceleration at the current path point, subscript i is the index of the current path point, p’ l , p’’ l are the first and second derivatives of the path with respect to the generalized arc length function, max x i+1 is the square of the maximum generalized velocity allowed at the i+ 1st path point, v max is the maximum linear velocity of the robot end-effector in the world coordinate frame, j is the index of the robot axis, J is the Jacobian matrix.​​ 3. The robot motion planning method of claim 1, wherein, When the minimum generalized velocity allowed by the current path point is obtained, the linear programming is described as min x s.t. 0 ≤ x ≤ ṡ max 2 min u i+1 ≤ u ≤ u max 0 ≤ j p’ ( l i ) 2 x≤ j ṗ max 2 0 ≤ j p’’ ( l i ) x+ j p’(l i )u≤ j ṗ max 0 ≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x ≤ v max 2 min x i+1 ≤ x+2(l i+1 −l i )u ≤ max x i+1 wherein x is the square of the generalized velocity, u is the generalized acceleration, ṡ max is the upper bound of the generalized velocity, u max denotes the upper bound of the generalized acceleration u , min u i is the lower bound of the generalized acceleration at the current path point with index i is the index of the current path point, p’ l , p’’ l are the first and second derivatives of the path with respect to the generalized arc length function, max x i+1 is the square of the maximum generalized velocity allowed for the i+ 1st path point, v max is the maximum linear velocity of the robot end-effector in the world coordinate system, j is the index of the robot axis, J is the Jacobian matrix.​​ 4. The robot motion planning method according to claim 2 or 3, characterized in that, Further, when the acceleration lower limit of the path point is determined, if the maximum generalized velocity allowed by the current path point is greater than the maximum generalized velocity corresponding to a previous path point, the acceleration lower limit of a next path point is updated according to the maximum generalized velocity allowed by the current path point.

5. The robotic motion planning method of claim 1, wherein, The linear programming is described as max u s.t. ṡ i 2 ≤ x ≤ ṡ i 2 min u i ≤ u ≤ max u i 0 ≤ j p’(l i ) 2 x≤ j ṗ max 2 0 ≤ j p’’ ( l i ) x+ j p’(l i )u≤ j ṗ max 0 ≤p’(l i ) T J(p(l i )) T J(p(l i ))p’(l i )x ≤ v max 2 min x i+1 ≤ x+2(l i+1 −l i )u ≤ max x i+1 wherein x is the square of the generalized velocity, u is the generalized acceleration, ṡ max is the upper bound of the generalized velocity, u max denotes the upper bound of the generalized acceleration u , min u i is the lower bound of the generalized acceleration at the current path point, subscript i is the index of the current path point, p’ l , p’’ l are the first and second derivatives of the path with respect to the generalized arc length function, max x i+1 is the square of the maximum generalized velocity allowed for the i+ 1st path point, v max is the maximum linear velocity of the robot end-effector in the world coordinate frame, ṡ i is the generalized velocity at the current path point, max u i is the upper bound of the generalized acceleration at the current path point, j is the index of the robot axis, J is the Jacobian matrix.​​ 6. The robotic motion planning method of claim 1, wherein, The judging whether there is an acceleration discontinuous point in the path point of the robot joint according to the generalized acceleration comprises: if an absolute value of a difference between a generalized acceleration of a current path point and a generalized acceleration of a previous path point is greater than a preset threshold, determining that the current path point is an acceleration discontinuous point.

7. The robotic motion planning method of claim 1, wherein, The determining of the function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the interpolated generalized velocity comprises: obtaining a time sequence corresponding to the robot joint according to the generalized arc length of each path point of the robot joint and the interpolated generalized velocity. The generalized arc length of the path point of the robot joint and the time sequence are fitted to obtain a function of the path with respect to time.

8. A robot motion planning apparatus characterized by comprising: The method comprises a data processing module, a linear programming module, an interpolation module and a path planning module. The data processing module is configured to obtain the generalized arc length of the path point of the robot joint and a function of the path with respect to the generalized arc length. The linear programming module is configured to obtain the generalized velocity interval of each path point of the robot joint according to the generalized arc length of the path point and the function of the path with respect to the generalized arc length. The linear programming module is configured to obtain the generalized velocity interval of each path point of the robot joint according to the generalized arc length of the path point and the function of the path with respect to the generalized arc length. The interpolation module is configured to determine whether there is an acceleration discontinuous point in the path point of the robot joint according to the generalized acceleration, and if there is, perform generalized velocity interpolation in the vicinity of the acceleration discontinuous point. The path planning module is configured to determine a function of the path of the robot joint with respect to time according to the generalized arc length of each path point of the robot joint and the interpolated generalized velocity.

9. A robot, characterized in that The storage medium stores program instructions, and the program instructions are executed by the processor to implement the robot motion planning method of any one of claims 1-7.

10. A storage medium, characterized by The storage medium stores program instructions, and the program instructions are executed by the processor to implement the robot motion planning method of any one of claims 1-7.

Images