A curvature constraint-based continuous pose velocity planning method for industrial robots

Abstract

The application provides a curvature constraint-based industrial robot continuous pose velocity planning method, comprising the following steps: determining velocity planning parameters by using numerical integration and curvature constraint, wherein the velocity planning parameters comprise curve length, parameter fitting equation, deceleration point and running speed limit; designing two kinds of velocity planning methods based on S-shaped velocity curve; segmenting a trajectory curve; and completing continuous pose velocity planning on each curve segment according to set initial and final velocities and acceleration and the two kinds of velocity planning methods.

Description

Technical Field

[0001] This invention relates to the field of industrial robot technology, and more specifically, to a method for continuous pose and velocity planning of industrial robots based on curvature constraints. Background Technology

[0002] Trajectory planning essentially explores the relationship between time and space, typically expressed as a parametric equation with time as the independent variable. This equation describes the relationship between a given moment and its desired position. In industrial robot control, velocity planning to achieve efficient and smooth motion is crucial. Current research in this area is still insufficient. Summary of the Invention

[0003] To address the technical problems mentioned above, this invention provides a method for continuous pose and velocity planning of industrial robots based on curvature constraints.

[0004] A method for continuous pose velocity planning of industrial robots based on curvature constraints, the method comprising:

[0005] The speed planning parameters are determined using numerical integration and curvature constraints. These parameters include curve length, parameter fitting equation, deceleration point, and operating speed limit.

[0006] Two velocity planning methods are designed based on the S-shaped velocity curve. The trajectory curve is segmented, and continuous pose velocity planning is completed on each segment according to the set initial and final velocities and accelerations and the two velocity planning methods mentioned above.

[0007] Optionally, the length of the B-spline curve can be calculated by integrating it.

[0008] The integral of the curve length is calculated using the compound Simpson's formula, as shown below:

[0009]

[0010] Where f(x) is the integrand, a and b are the left and right endpoints of the integration interval, h = (ba) / n, n is the number of intervals, and x i = a + i·h.

[0011] Optionally, the parameter fitting equation is determined in the following manner:

[0012] The curve parameters are fitted using an inverse function of the curve parameters based on the curve length constructed using a cubic Hermite polynomial. The interpolation parameters are determined based on the cumulative displacement at each time point, and the corresponding trajectory points are then calculated, including:

[0013] By constructing any u∈[u j uj+1 The inverse function u(L) of the curve length L corresponding to the curve parameter u within the interval, thus making L∈[L j L j+1 Establish a mapping relationship between the curve parameter u and the j-th segment u. j The expression for the (L) function is as follows:

[0014] u j (L)=A+B(LL j )+C(LL j ) 2 +D(LL j ) 2 (LL j+1 )

[0015] In the formula, A, B, C, and D are polynomial coefficients, as follows:

[0016] A = u j B=u′ j ,

[0017]

[0018] L j ——In [0,u j The length of the curve integral over the interval.

[0019] Optionally, the curvature extrema can be determined by analyzing the changes in path curvature, and the curvature extrema that exceed the constraints can be used as deceleration points.

[0020] Optionally, the running speed limit is specifically as follows:

[0021]

[0022] Optionally, the two speed planning methods are an S-shaped speed planning method with given motion parameter constraints and an S-shaped speed planning method with given running time constraints.

[0023] The beneficial effects of this invention are as follows:

[0024] After obtaining the robot's spatial geometric motion trajectory, this invention designs a continuous pose velocity planning scheme based on curvature constraints. The trajectory length is calculated using the composite Simpson's formula and the derivative of a cubic B-spline curve. A cubic Hermite polynomial is used to fit the curve parameters, allowing interpolation parameters to be determined based on cumulative displacement after velocity planning. Curvature formulas for the plane and spatial parametric equations are given by defining curvature and arc differential, and velocity limits and deceleration points are calculated using curvature constraints. This invention also designs a velocity planning method to calculate all parameters of the S-shaped velocity curve under two conditions: known motion parameter constraints and running time constraints, to meet the requirements of S-shaped velocity curve-based velocity planning under different input conditions. Attached Figure Description

[0025] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0026] Figure 1 This is a flowchart of a continuous pose and velocity planning method for industrial robots based on curvature constraints, as disclosed in an embodiment of the present invention.

[0027] Figure 2 This is a schematic diagram of the curvature definition disclosed in an embodiment of the present invention;

[0028] Figure 3 This is a schematic diagram of the arc differential of the plane parametric equation disclosed in an embodiment of the present invention;

[0029] Figure 4 This is a schematic diagram of the tangent angle of the plane parametric equation disclosed in an embodiment of the present invention;

[0030] Figure 5 This is an example of a curvature change curve disclosed in an embodiment of the present invention;

[0031] Figure 6 This is a flowchart of an S-shaped speed planning method with a given speed limit disclosed in an embodiment of the present invention. Detailed Implementation

[0032] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0033] Figure 1 This invention discloses a method for continuous pose and velocity planning of an industrial robot based on curvature constraints, the method comprising:

[0034] The speed planning parameters are determined using numerical integration and curvature constraints. These parameters include curve length, parameter fitting equation, deceleration point, and operating speed limit.

[0035] Two velocity planning methods are designed based on the S-shaped velocity curve. The trajectory curve is segmented, and continuous pose velocity planning is completed on each segment according to the set initial and final velocities and accelerations and the two velocity planning methods mentioned above.

[0036] Optionally, the length of the B-spline curve can be calculated by integrating it.

[0037] The integral of the curve length is calculated using the compound Simpson's formula, as shown below:

[0038]

[0039] Here, f(x) is the integrand, and a and b are the left and right endpoints of the integration interval, respectively.

[0040] To avoid high-order instability caused by simply increasing the order of the quadrature formula, the integration interval can be divided into multiple subintervals. The integral value of each subinterval can be calculated using a lower-order numerical integration formula, and these integral values ​​are then summed to obtain an approximate integral value for the entire interval. This composite quadrature method can reduce errors and improve computational accuracy. Therefore, Simpson's formula is extended to a composite Simpson's formula:

[0041]

[0042] Where h = (ba) / n, n is the number of intervals divided, x i = a + i·h.

[0043] B-spline curves are functions of normalized parameters, while actual control commands are sent at fixed time intervals. Therefore, after speed planning is completed, it is necessary to calculate a series of trajectory points at fixed time intervals.

[0044] Optionally, this invention uses a cubic Hermite polynomial to construct an inverse function of the curve parameters based on the curve length to fit the curve parameters, determines the interpolation parameters based on the cumulative displacement corresponding to the time point, and then calculates the corresponding trajectory points. Specifically, by constructing any u∈[u j u j+1 The inverse function u(L) of the curve length L corresponding to the curve parameter u within the interval, thus making L∈[L j Lj+1 Establish a mapping relationship between the curve parameter u and the j-th segment u. j The expression for the (L) function is as follows:

[0045] u j (L)=A+B(LL j )+C(LL j ) 2 +D(LL j ) 2 (LL j+1 (3)

[0046] In the formula, A, B, C, and D are polynomial coefficients;

[0047] L j ——In [0,u j The length of the curve integral over the interval.

[0048] The above equation has four unknown coefficients, requiring four equations to solve. Since L... j and L j+1 Corresponding to u j and u j+1 With two endpoints, we can first obtain the following two equations:

[0049]

[0050] Since u(L) is a function of the curve parameter with respect to the curve length, and is an inverse function of the curve length with respect to the curve parameter L(u), their derivatives are inverses of each other, resulting in the following two equations.

[0051]

[0052]

[0053] By combining the above equations, we can obtain the following four coefficients:

[0054]

[0055] At this point, the curve parameters can be constructed as a function of the curve length. After obtaining the displacement L at each moment according to the velocity planning, based on the [L] to which L belongs... j L j+1 Substitute the corresponding curve parameter u into the function u of the curve length L. j (L), then the corresponding u value can be calculated, and the interpolation point coordinates at the corresponding time can be generated:

[0056]

[0057] Optionally, the curvature extrema are determined by analyzing the changes in path curvature, and the curvature extrema that exceed the constraints are taken as deceleration points;

[0058] The extreme points of curvature are determined as follows:

[0059] During motion, regions with greater curvature on a curve typically require lower speeds to ensure stability and safety. Therefore, to improve trajectory planning performance, it's necessary to consider the curvature of the curve and plan segmented speeds accordingly. Curvature, as a mathematical tool for quantifying the curvature of a curve, can describe the curvature characteristics of the curve, such as... Figure 2 As shown.

[0060] Suppose that curve C has a tangent line at every point, and the direction of the tangent line changes continuously as the point on the curve moves; that is, curve C is... 1 Continuous. M0 and M are the start and end points of arc segment s, respectively, and the inclination angle of the tangent at M is α. M′ corresponds to arc s+Δs, and the inclination angle of the tangent at M′ is α+Δα. Arc segment The length of the tangent is |Δs|, and the angle through which the tangent turns when moving from M to M′ is |Δα|. Then the mean curvature is... The expression is

[0061]

[0062] average curvature The limit of this is called the curvature K, i.e.

[0063]

[0064] The B-spline curves used in this invention are in parametric equation form; therefore, the curvature formula for the parametric equation is explained as follows:

[0065] First consider the plane parametric equations, such as Figure 3 As shown, let the parametric function be C(u) = (x(u), y(u)), a ≤ u ≤ b, and M and M′ be two adjacent points on the curve. The arc differential expression of arc s is obtained as follows:

[0066]

[0067] Suppose that the plane parametric curve is second-differentiable, such as Figure 4 Let the tangent angle at point M of the curve be α, and the unit tangent vector at point M be T. α It is also the derivative of the curve with respect to the arc length s, that is...

[0068]

[0069] So there are

[0070]

[0071] When Δu→0, Δs→0, Δα→0, therefore we have

[0072]

[0073] Therefore, according to equation (10), the curvature at point M can be expressed as:

[0074]

[0075] After simplification, the curvature expression of the plane parametric equation is obtained as follows:

[0076]

[0077] Generalizing this to the second-differentiable space parameter curve C(u)=(x(u), y(u), z(u)), a≤u≤b, we can obtain the expression for the curvature of the space parameter curve as follows:

[0078]

[0079] in,

[0080]

[0081] Verification reveals that when a curve transforms into a planar curve, the curvature formula for a spatial parametric curve degenerates into the curvature formula for a planar parametric curve. An example of the curvature variation curve for a cubic B-spline curve is given below. Figure 5 As shown. Observation Figure 5 As can be seen, the curvature of the cubic B-spline curve is continuous and has extreme points, so the velocity planning parameters can be set based on the curvature.

[0082] Optionally, the running speed limit can be determined as follows:

[0083] In curvilinear motion, the actual operating speed is limited by centripetal acceleration. Centripetal acceleration is the acceleration generated by the curvature of the curve, which affects the motion process. Therefore, it is necessary to consider the limitation of speed caused by centripetal acceleration due to curvature.

[0084] Defining the spatial parameter curve C(t) with time t as the parameter, we can obtain the formula for velocity:

[0085]

[0086] In the formula, β(t) is the unit vector of the tangent direction, and β(t) = C′(t) / ‖C′(t)‖.

[0087] The rate is

[0088]

[0089] In the formula, s is the arc length parameter of the curve.

[0090] Differentiating the velocity expression yields the acceleration expression as follows:

[0091]

[0092] According to the chain rule of differentiation, we can summarize as follows:

[0093]

[0094] According to the definition of curvature, the rate of change of the unit tangent vector of a curve can also represent the degree of curvature of the curve:

[0095]

[0096] Therefore, the acceleration formula becomes:

[0097]

[0098] Where γ(t) is the unit vector in the direction of the derivative of the unit vector β(t) in the tangent direction, γ(t)=β′(t) / ‖β′(t)‖, which is also the unit vector in the direction of the curve normal.

[0099] Therefore, it can be seen that the component of tangential acceleration a β (t) is

[0100]

[0101] The acceleration component along the normal direction is a γ (t)=‖C′(t)‖ 2 K.

[0102] If the maximum acceleration that an industrial robot can provide is a max Then the constraints that need to be satisfied are:

[0103]

[0104] At this point, the curve length, curvature extreme points, and corresponding running speed limits have been obtained. Based on these, the curve can be segmented, and speed planning can be performed on each segment according to the set initial and final velocities and accelerations.

[0105] Optionally, two velocity planning methods are used to complete continuous pose velocity planning, as follows:

[0106] 1. S-shaped velocity planning method with given motion parameter constraints, refer to Figure 6 As shown:

[0107] (1) Given that there may be differences in the magnitude relationship between the initial and final positions, that is, there are cases where q0 < q1 or q0 > q1, such differences will directly affect the formation of the final trajectory curve. To solve this problem, the present invention adopts a more general method to set the starting and ending positions and velocities, denoted as For trajectory calculation, the following conversions are performed on the original data:

[0108]

[0109] Among them Similarly, for constraint conditions such as velocity, acceleration, and jerk, corresponding conversion operations also need to be performed, denoted as

[0110]

[0111] (2) First, it is necessary to determine whether the specified maximum velocity can be reached. Assume that the maximum velocity is reachable, that is, v lim = v max . Then, evaluate whether the maximum acceleration (a max or a min = -a max ) is a feasible parameter: If then a max is considered unreachable. Similarly, if then a min is also considered unreachable.

[0112] Then calculate the acceleration and deceleration section times. In the case where a max is determined to be unreachable, we can calculate the initial acceleration section time T max based on v j1 . At this time, the time calculation method for the acceleration stage is as follows:

[0113]

[0114] If a max is reachable, then by using a max and j max to calculate T j1 , the time for the acceleration stage can be calculated according to the following method:

[0115]

[0116] If a min is unreachable, then calculate the T max value through v j2 and v1, and thus the total time for the deceleration stage can be calculated according to the following method:

[0117]

[0118] If a min If reachable, then by applying a max and j max Calculate T j2 Therefore, the total time of the deceleration phase can be calculated using the following method:

[0119]

[0120] After determining the time required for acceleration and deceleration, the time for the constant speed phase can be further calculated:

[0121]

[0122] If T v If v > 0, it indicates that the assumption that the maximum speed can actually be reached is true, i.e., v lim =v max At this point, there is a uniform speed running phase. Through organization and calculation, T can be obtained. a T v T d T j1 T j2 Then proceed directly to step 4 to calculate the trajectory.

[0123] Conversely, if T v ≤0 indicates that the assumption of reaching the set maximum speed is not valid, i.e., v lim <v max There will be no uniform running time. In this case, set T... v =0, then proceed to step 3 to recalculate the time for each stage.

[0124] (3) When the system cannot reach the predetermined maximum speed, i.e., T v =0, at which point it is assumed that the system can reach the maximum and minimum acceleration values, and then the calculation is performed as follows:

[0125]

[0126] in,

[0127]

[0128] After obtaining the acceleration time T a and deceleration time T d Next, we need to consider the various scenarios for these two durations separately:

[0129] If T a <0||T d<0 means T a or T d At least one of them is negative, which means it may only contain acceleration or deceleration segments. In this case, the following should be done:

[0130] If T a A value less than 0 indicates that the system has no acceleration phase, only a deceleration phase. In this case, T should be set to 0. a =T j1 =0, and calculate the time parameters of the deceleration phase as follows:

[0131]

[0132] If T d If T < 0, it indicates that there is no deceleration phase, only an acceleration phase. In this case, T should be set to 0. d =T j2 =0, and calculate the time parameters for the acceleration phase as follows:

[0133]

[0134] Organize and calculate T a T v T d T j1 T j2 Then, proceed directly to the fourth step to perform trajectory calculation.

[0135] If T a <0||T d The condition < 0 is not met, i.e., T a or T d If all numbers are positive, then T needs to be evaluated. a With T j1 T d With T j2 The relationship between them:

[0136] If T a ≥2T j1 &&T d ≥2T j2 This indicates that the acceleration is limited in both the acceleration and deceleration phases. In this case, T can be directly calculated using equation (33). a T v T d T j1 T j2 Parameters, then continue to step 4 to calculate the trajectory.

[0137] If condition T a ≥2T j1 &&T d ≥2Tj2 If not satisfied, it means that T a ≤2T j1 ||T d ≤2T j2 This means that during the system's acceleration or deceleration phases, there is at least a period where maximum acceleration cannot be achieved. In this case, parameter setting becomes extremely complex, and an approximate solution strategy can be used to simplify the problem. Specifically, this involves gradually reducing 'a'. max The value, for example, setting a max =γa max (0<γ<1), until T is satisfied a ≥2T j1 &&T d ≥2T j2 The conditions are met. Once the conditions are met, the corresponding T is calculated according to equation (33). a T v T d T j1 T j2 The value is determined, and based on this, step 4 is performed to continue trajectory calculation.

[0138] (4) When T is obtained a T v T d T j1 T j2 After that, q0, v0, j max , v lim j min , q1 and v1 are also known, so the expressions for each motion parameter can be calculated according to equations (4-53) to (4-59).

[0139] (5) Finally, the σ pair is used to calculate the resulting profile. Perform another conversion:

[0140]

[0141] 2. S-curve velocity planning method with given running time constraints:

[0142] Because S-shaped velocity programming presents a wide variety of scenarios, given constraints and a pre-set running time, the displacements achievable through the S-shaped velocity programming method are categorized into the following five curves, L1 to L5, for detailed analysis:

[0143] (1) Curve L1 is the velocity curve representing the maximum displacement achievable within a running time T. The acceleration, constant speed, and deceleration times at this point are respectively... and

[0144] (2) The L2 curve considers the minimum displacement that can be achieved under the running time T. First, it calculates the deceleration from the initial velocity v0 to the minimum velocity v. min And the time it takes to accelerate from the minimum speed to the final speed v1, where the minimum speed v min This is obtained by applying the S-curve velocity planning algorithm with the target speed limit set to zero and changing the sign. Then, the remaining time is allocated to the uniform speed segment maintaining the minimum speed, thereby calculating the minimum displacement over time T. The deceleration time is defined as... The uniform speed time is acceleration time is

[0145] (3) Curve L3 accelerates directly from an initial velocity v0 to a final velocity v1, with a running time of... Then it runs at a constant speed of v1.

[0146] (4) The L4 curve is the direct acceleration from the initial velocity v0 to the entire running time T to v1;

[0147] (5) The L5 curve initially runs at a constant speed with an initial velocity v_0. Upgrade to v1, then speed up the time. Final velocity v1;

[0148] By comparing the target displacement S with the displacements corresponding to the five curves above, the displacement interval is determined, and velocity planning is implemented under different conditions. The specific steps are as follows:

[0149] (1) Using the S-shaped velocity programming algorithm, calculate the maximum displacement S corresponding to curves L1 and L2. max and minimum displacement S min Here, we also need to determine the maximum operating speed of the L1 curve. It can be obtained in the following way.

[0150] First, assume Discuss whether the maximum speed limit can be reached and calculate the acceleration time. and deceleration time if This means that the speed limit can be reached at this point. Calculation complete.

[0151] if This indicates that the maximum speed limit cannot be reached, and there is no constant speed segment. In this case, the possible maximum speed needs to be calculated based on the initial or final speed, by limiting acceleration or jerk. Since the default final speed is higher, it is used as a reference first. If the initial speed is higher, the case based on its actual speed is considered first. According to the acceleration characteristics of the S-curve, the maximum speed is set. for:

[0152]

[0153] if This indicates that acceleration can reach a. max Then calculate the acceleration time. and deceleration time Then determine whether If true, then at this time... Internal Search until Based on the time relationship, we can obtain the time during which the additional acceleration during the acceleration and deceleration phases is constant:

[0154]

[0155] if Let's consider planning based on the starting speed, and set the maximum speed as follows. for:

[0156]

[0157] Similarly, if Then at this time Internal Search until Conversely, in Search within. The calculation approach is the same.

[0158] Maximum displacement S max and minimum displacement S min The calculation formula is as follows:

[0159]

[0160] Determine whether the target displacement S is within the reachable range. If S∈[S min S max This indicates that under the current constraints, the target speed planning can be completed, and we can proceed to step 2 to continue speed planning.

[0161] like This indicates that speed planning cannot be completed under the current constraints.

[0162] (2) Calculate the displacements s3, s4, and s5 corresponding to curves L3, L4, and L5. The calculation formulas are as follows:

[0163]

[0164] Based on the displacements corresponding to the five curves, the displacement region can be divided into four parts, and the displacement space can be divided into four intervals, which will be discussed separately according to the situation.

[0165] (3) When S∈[S min At point S5, the velocity trajectory lies between L2 and L5, and each time segment matches the L2 curve. Based on the curve symmetry, the minimum displacement and velocity are... The relationship formula is:

[0166]

[0167] Thus, the minimum speed is obtained. The formula for calculation is:

[0168]

[0169] Calculated Next, we will further explore the changes in acceleration during acceleration and deceleration. The acceleration and deceleration segments are similar, so we will take the deceleration segment as an example. First, we need to determine if a constant acceleration, i.e., uniform deceleration segment, exists. Since the jerk remains constant during S-shaped velocity planning, we can determine the existence of a uniform deceleration segment by the magnitude of the jerk. Let's assume the deceleration segment time of the L2 curve is... Furthermore, since there is no uniform deceleration segment within the deceleration phase, according to the principle that the area enclosed by the triangle on the acceleration curve represents the velocity, the acceleration J... d The relationship between the change in velocity and the velocity can be expressed by the following formula:

[0170]

[0171] The jerk j at this point is:

[0172]

[0173] Since at this time S∈[S min S5), the target velocity curve must lie between these two curves, multiply the obtained j by With constraint a max In comparison. Because of constraint a max It's a critical value, equivalent to a value within a range that just accelerates to a. max And deceleration period There is no curve to compare with in the uniformly decelerated section to determine its own position. If Explain the deceleration period When there is no uniform deceleration phase, using acceleration j can ensure that the displacement target is achieved without exceeding the limit. Therefore, the non-zero time T of acceleration j during the deceleration phase... j2 The expression is:

[0174]

[0175] if Explain the deceleration period A uniform deceleration phase is needed to complete the displacement under constraints. Based on the principle that the area enclosed by the triangle formed by the acceleration curve represents the velocity, the time T during the deceleration phase when the acceleration j is not zero is... j2 The relation is:

[0176]

[0177] Therefore, T j2 The calculation expression is:

[0178]

[0179] Similarly, the timing of the acceleration phase can be determined.

[0180] (4) When S∈[S5, S4), the velocity has only two states: constant speed and acceleration. The constant speed time interval T v The relationship can be obtained from the area under the velocity curve:

[0181]

[0182] Sorting can yield T v The expression is:

[0183]

[0184] So, the acceleration period T a You can also get:

[0185] T a =TT v (52)

[0186] acceleration phase T j1 The solution can be found by referring to the method in step 3.

[0187] (5) When S∈[S4, S3), the velocity has only two states: acceleration and constant velocity. The constant velocity time T is obtained by following the method in step 4. v and acceleration period T a The relation is:

[0188]

[0189] acceleration phase T j1 The solution can be found by referring to the method in step 3.

[0190] (6) When S∈[S3, S MAX At this point, the velocity curve is located between L1 and L3. Position the velocity curve so that the time nodes for acceleration, constant speed, and deceleration in the planned time are the same as those of the L1 curve. Then, obtain the maximum velocity value using the method described in step 3. The formula is:

[0191]

[0192] The method for calculating the time corresponding to the acceleration and deceleration phases is the same as in step 3.

[0193] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for continuous pose and velocity planning of industrial robots based on curvature constraints, characterized in that, The method includes: The speed planning parameters are determined using numerical integration and curvature constraints. These parameters include curve length, parameter fitting equation, deceleration point, and operating speed limit. Two velocity planning methods are designed based on the S-shaped velocity curve. The trajectory curve is segmented, and continuous pose velocity planning is completed on each segment according to the set initial and final velocities and accelerations and the two velocity planning methods mentioned above. The length of the curve is calculated by integrating the B-spline curve: The integral of the curve length is calculated using the compound Simpson's formula, as shown below: ； in, It is the integrand. and These are the left and right endpoints of the integration interval, respectively. , It is the number of intervals divided. ; The parameter fitting equation is determined in the following manner: The curve parameters are fitted using an inverse function of the curve parameters based on the curve length constructed using a cubic Hermite polynomial. The interpolation parameters are determined based on the cumulative displacement at each time point, and the corresponding trajectory points are then calculated, including: By constructing arbitrary Curve parameters within the interval Corresponding curve length inverse function Thus With curve parameters Establish mapping relationship, the first Section The function expression is as follows: ； In the formula, A, B, C, and D are polynomial coefficients, as follows: ； --exist The integral of the curve length over the interval; This represents curve parameters. Regarding curve length The derivative of The value at that location, This represents curve parameters. Regarding curve length The derivative of The value at; The two speed planning methods are the S-shaped speed planning method with given motion parameter constraints and the S-shaped speed planning method with given running time constraints.

2. The method for continuous pose and velocity planning of an industrial robot based on curvature constraints according to claim 1, characterized in that: By analyzing the changes in path curvature, the curvature extrema are determined, and the curvature extrema exceeding the constraints are taken as deceleration points.

3. The method for continuous pose and velocity planning of an industrial robot based on curvature constraints according to claim 2, characterized in that: The specific operating speed limits are as follows: ； in, for The component of tangential acceleration at any given time. for The acceleration component along the normal direction at any given moment. This is the maximum acceleration.

Images