A robot strict path time-optimal trajectory planning method and system based on arc length segmentation optimization

Abstract

The application provides a robot strict path time-optimal trajectory planning method and system based on arc length segmentation optimization, and belongs to the technical field of robot control and trajectory planning. The method comprises the following steps: according to a preset Cartesian space path, a continuous mapping relationship between a mechanical arm end pose and joint position along a path parameter and geometric derivatives of each order of the path parameter relative to the path parameter are established; the path parameter is segmented, and a basic motion law of the path parameter changing with time is defined based on each segmented motion time interval; joint space high-order dynamics constraints are converted into algebraic inequality groups dependent on pseudo-motion state variables, geometric derivatives of each order and each segmented motion time interval; a nonlinear programming model is constructed and an optimal time interval sequence is solved, a continuous function of the path parameter with respect to time is reconstructed, and a position, velocity and acceleration trajectory of the mechanical arm joint space is generated in combination with the continuous mapping relationship. The application strictly tracks the path, is time-optimal and satisfies high-order dynamics constraints.

Description

Technical Field

[0001] This application belongs to the field of robot control and trajectory planning technology, specifically involving a robot strict path time optimal trajectory planning method and system based on arc length segmentation optimization. Background Technology

[0002] In industrial automation and high-precision manufacturing, robotic arms need to maximize their movement speed and minimize cycle time while strictly adhering to a pre-set Cartesian space path, thereby achieving efficient production. Existing technologies primarily employ time-optimal trajectory planning methods, including numerical integration based on the Pontrypaniform maximum principle and trajectory planning methods based on convex optimization.

[0003] Numerical integration methods typically search for feasible boundaries of joint velocities and accelerations in the phase plane and solve for the time-optimal trajectory through inverse integration. This method is effective when only velocity and acceleration constraints exist, but its state space dimension is limited, making it difficult to handle higher-order dynamic constraints such as jerk. Once higher-order constraints are introduced, the system dimension increases dramatically, leading to difficulty in solving the problem or even making it unsolvable. In practical applications, this can easily cause rigid impacts or residual vibrations in the robotic arm.

[0004] Convex optimization methods model the trajectory planning problem as a convex optimization problem, theoretically leading to a globally optimal solution. However, this method requires converting nonlinear dynamic constraints into a specific convex form, resulting in a complex modeling process. Furthermore, introducing new constraints necessitates re-deriving the optimization model, leading to poor versatility. In addition, convex optimization solutions are computationally expensive, making them unsuitable for real-time performance or practical engineering applications.

[0005] In summary, existing technologies have significant shortcomings in handling high-order smoothness constraints, and suffer from complex modeling, poor scalability, and an inability to fully leverage the dynamic performance of the robotic arm while strictly adhering to Cartesian paths. Therefore, there is an urgent need for a time-optimal trajectory planning method that can efficiently handle high-order dynamic constraints under strict Cartesian path constraints. Summary of the Invention

[0006] In a first aspect, embodiments of this application provide a robot's strictly time-optimal trajectory planning method based on arc length segmentation optimization, comprising the following steps: S1. Based on a preset Cartesian space path, establish a continuous mapping relationship between the end-effector pose and joint position along the path parameters, and determine the geometric derivatives of the continuous mapping relationship with respect to the path parameters. S2. Divide the entire path parameters into several continuous segments, and use the motion time interval corresponding to each segment as the variable to be optimized. Predefine the basic motion law of the path parameters changing with time for each segment. S3. Define pseudo-motion state variables to characterize the speed of path progress, and transform the higher-order dynamic constraints of the joint space into a set of algebraic inequalities that depend only on the pseudo-motion state variables, geometric derivatives of each order, and the motion time interval of each segment based on the chain rule. S4. Construct and solve a nonlinear programming model with the goal of minimizing the sum of all segmented motion time intervals and a set of algebraic inequalities as constraints to obtain the optimal time interval sequence. S5. Based on the optimal time interval sequence and the basic motion law, reconstruct the continuous function of the path parameters over time, and combine the continuous mapping relationship to generate the full trajectory of the position, velocity and acceleration of the robotic arm joint space.

[0007] Furthermore, the specific steps of step S1 are as follows: S11. Perform dense resampling on the preset Cartesian space path, using the path parameters. Using [variable name] as the independent variable, obtain discrete end-effector pose sequences. ,in, Let j be the position vector of the j-th discrete point. Let j be the attitude vector of the j-th discrete point; S12. Perform inverse kinematics calculations on each discrete end-effector pose to obtain the corresponding joint position sequence. , n is the number of joints in the robotic arm. Let j be the joint position vector of the j-th discrete point; S13. Employ spline interpolation or polynomial fitting methods, based on... Construct joint positions with respect to path parameters continuous mapping function ; S14. For the continuous mapping function Determine the first-order geometric derivative using analytical or numerical differentiation. Second-order geometric derivative and higher-order geometric derivatives, and stored as with respect to path parameters. The lookup table or function handle is provided for direct use in subsequent steps.

[0008] Furthermore, the specific steps of step S2 are as follows: S21. Pass path parameters The entire range of values ​​from the starting point to the ending point is divided into N consecutive segments, and the path parameter increments for each segment are... Given that the time interval of the i-th segment is defined... Let i be the variable to be optimized, i = 1, 2, ..., N; S22. The variation of the predefined path parameters for each segment with time t is an nth-degree polynomial:

[0009] Where n is the number of terms in the polynomial, and n≥3; These are polynomial coefficients, analytically determined by the continuity boundary conditions of position, velocity, and acceleration at the beginning and end of the k-th segment; S23. Establish polynomial coefficients Time interval between segments of motion The implicit mapping relationship is uniquely determined by a system of linear equations constituting continuous boundary conditions; based on the implicit mapping relationship, the adjustment... This allows for proportional scaling of the magnitudes of the time derivatives of the path parameters within the corresponding segments, thus avoiding explicit calculation of polynomial coefficients during the optimization process.

[0010] Furthermore, the specific steps of step S3 are as follows: S31. Based on path parameters Pseudo-motion state variables are defined as state variables representing the speed of path progress. These pseudo-motion state variables include pseudo-velocities. pseudo-acceleration and pseudo-accelerometer ; S32. Based on the chain rule, the joint velocity... Joint acceleration Represented as a function of geometric derivatives and pseudo-motion state variables:

[0011] And further accelerate the joints Represented as:

[0012] S33. Transform the joint velocity constraint into a constraint about... Algebraic inequalities:

[0013] in, The first geometric derivative determined in step S14, This is the joint velocity limit vector; S34 transforms the joint acceleration constraint and the joint jerk constraint into constraints related to... , , Algebraic inequalities; S35. Constrain joint torques Transforming the robot dynamics equations into... , , Algebraic inequalities:

[0014] in, For the quality matrix, For the Coriolis and centrifugal force matrices, The gravity vector This is the limit vector of joint torque; S36. Connect the pseudo-motion state variables with the time intervals of each segmented motion. Related: Based on the nth-degree polynomial in step S22, derive... , , The expression within the segment, and further rewrite all algebraic inequalities in steps S33 to S35 as depending only on And the forms of geometric derivatives of each order, to complete the algebraic dimensionality reduction mapping of higher-order dynamic constraints.

[0015] Furthermore, the algebraic inequalities of joint velocity constraints in step S33 In, the first-order geometric derivative Jacobian matrix Cartesian position derivative satisfy The inequality of the joint velocity constraint is equivalent to the form expressed by the Jacobi inverse.

[0016] Furthermore, the algebraic inequalities for joint acceleration constraints and joint jerk constraints in step S34 are obtained through the following chain expansions: ,

[0017] ,

[0018] And , , The values ​​within the segment are represented as follows: Explicit functions; in, Pseudo-speed, It is pseudo-acceleration. It is a pseudo-jerk. The limit of the second derivative. It is the limit value of the third derivative.

[0019] Furthermore, the specific steps of step S4 are as follows: S41. Construct a nonlinear programming model, setting the objective function of the nonlinear programming model to the sum of the time intervals of all segmented motions:

[0020] Where N is the number of segments; S42. Add the system of algebraic inequalities obtained in step S3 as constraints to the model. The constraints include joint velocity constraints, joint acceleration constraints, joint jerk constraints, torque constraints, and pseudo-velocity nonnegativity constraints. and time positive definiteness constraints ; S43. Solving the nonlinear programming model using a hybrid iterative algorithm: Perform the global search phase, with For each individual, a genetic algorithm is applied to perform global optimization within the feasible region to obtain an approximate optimal solution as the initial seed. In the local optimization phase, using the output of the genetic algorithm as the initial value, a sequential quadratic programming algorithm is applied for a fine search, iterating until the Karush-Kuhn-Tucker path parameter conditions are met, and the optimal time interval sequence is output. .

[0021] Furthermore, step S43 is detailed as follows: S431. Perform a global search using the genetic algorithm: The time interval of each segment of motion For individual encoding, real number encoding or binary encoding is used, and population size, crossover probability, and mutation probability are set; where N is the number of segments; The reciprocal of the objective function of the nonlinear programming model As the fitness function, the algebraic inequalities in step S42 are used as constraints. For individuals that do not meet the constraints, the fitness is corrected by introducing a penalty term that is positively correlated with the amount of constraint violation. Iteratively perform selection, crossover, and mutation operations until a preset maximum number of generations is reached or the fitness improvement is less than a set threshold. Then, output the best individual in the current population as the global approximate optimal solution. ; S432. Perform local optimization of sequential quadratic programming: The best individual output by the genetic algorithm As initial values ​​for the sequential quadratic programming algorithm; Set the convergence accuracy and maximum number of iterations; The Hessian matrix approximation of the Lagrange function is updated using a quasi-Newton method, transforming the nonlinear programming problem into a series of quadratic programming subproblems that are solved iteratively. In each iteration, based on the current Calling the pre-stored geometric derivatives from step S14 and the pseudo-motion state variables established in step S36, and The explicit relationship between the constraint violation degree and the gradient of the objective function is calculated. The iteration terminates when the Karush-Kuhn-Tucker condition is met or the iteration step size is less than the convergence accuracy, and outputs the optimal time interval sequence. .

[0022] Furthermore, the specific steps of step S5 are as follows: S51. Based on the optimal time interval sequence Accumulate to generate global time nodes k=0,1,…,N, where ; S52. Based on the predefined polynomial model and optimal time interval sequence in step S22, reconstruct the continuous smooth function of the path parameters over time. Quartic spline interpolation is used to ensure It has continuous second derivatives and the interpolation error is controllable; S53. Call the geometric derivative lookup table or function stored in step S14, and combine it with the continuous mapping relationship. Inverse calculation of the entire joint position trajectory:

[0023] S54. Calculate joint velocity and joint acceleration trajectories based on the chain rule:

[0024] S55. Outputs the position, velocity, and acceleration trajectory of the robotic arm joints throughout the entire process, for controller execution or offline simulation.

[0025] Secondly, embodiments of this application also provide a robot strictly path-time optimal trajectory planning system based on arc length segmentation optimization, comprising: The path geometric feature extraction module is used to establish a continuous mapping relationship between the end pose of the robotic arm and the joint position along the path parameters based on a preset Cartesian space path, and to determine the geometric derivatives of the continuous mapping relationship with respect to the path parameters. The time-domain discretization module is used to divide the entire path parameters into several continuous segments, with the motion time interval corresponding to each segment as the variable to be optimized, and to predefine the basic motion law of the path parameters changing with time for each segment. The algebraization module of dynamic constraints is used to define pseudo-motion state variables that characterize the speed of path progress. Based on the chain rule, it transforms the high-order dynamic constraints of the joint space into a set of algebraic inequalities that depend only on the pseudo-motion state variables, geometric derivatives of each order, and the motion time interval of each segment. The optimization solution module is used to construct and solve a nonlinear programming model with the goal of minimizing the sum of all segmented motion time intervals and a set of algebraic inequalities as constraints, to obtain the optimal time interval sequence. The trajectory reconstruction module is used to reconstruct the continuous function of path parameters over time based on the optimal time interval sequence and the basic motion law, and generate the full trajectory of position, velocity and acceleration of the robotic arm joint space by combining the continuous mapping relationship.

[0026] As can be seen from the above technical solutions, this application has the following advantages: This application provides a robot strict path time optimal trajectory planning method and system based on arc length piecewise optimization. By transforming the high-dimensional nonlinear dynamic constraint problem into a system of algebraic inequalities that depend only on the geometric derivatives of path parameters and time interval variables, and constructing a nonlinear programming model with the goal of minimizing the total running time, this method fully explores the dynamic performance limits of the robotic arm joints while strictly maintaining the end effector's motion along a preset Cartesian path. This application ensures that the trajectory strictly follows the given Cartesian path throughout by establishing a continuous mapping between the end effector pose and joint position with respect to path parameters. It uniformly handles multiple high-order dynamic constraints such as velocity, acceleration, jerk, and joint torque, avoiding rigid impacts and residual vibrations. It employs pre-calculated geometric derivatives and time discrete optimization strategies to avoid repeatedly solving inverse kinematics and dynamic differential equations online. This application achieves that adding constraints within the nonlinear programming framework only requires adding algebraic inequalities, without reconstructing the model. This application combines a hybrid iterative strategy of a genetic algorithm for global search and a sequential quadratic programming strategy for local fine-tuning, balancing convergence and optimality. Attached Figure Description

[0027] To more clearly illustrate the technical solution of this application, the accompanying drawings used in the description will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0028] Figure 1 This is a flowchart illustrating the robot's strictly time-optimal trajectory planning method based on arc length segmentation optimization according to the present invention.

[0029] Figure 2 This is a schematic diagram of the robot's strictly time-optimal trajectory planning system based on arc length segmentation optimization according to the present invention. Detailed Implementation

[0030] The various embodiments of this disclosure will be described more fully in the following detailed description of the specific steps of the robot's rigorous path-time optimal trajectory planning method based on arc length segmentation optimization. This disclosure may have various embodiments, and adjustments and changes may be made therein. However, it should be understood that there is no intention to limit the various embodiments of this disclosure to the specific embodiments disclosed herein, but rather this disclosure should be understood to cover all adjustments, equivalents, and / or alternatives falling within the spirit and scope of the various embodiments of this disclosure.

[0031] This embodiment provides a robot strict path time optimal trajectory planning method based on arc length segmentation optimization. Under the premise of strictly maintaining the end effector's movement along a preset Cartesian path, it achieves global time optimal trajectory generation under multiple constraints such as joint velocity, acceleration, and torque.

[0032] 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. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0033] Please see Figure 1 The diagram shows a flowchart of a robot's strictly time-optimal trajectory planning method based on arc length segmentation optimization in a specific embodiment. The method includes the following steps: S1. Based on a preset Cartesian space path, establish a continuous mapping relationship between the end-effector pose and joint position along the path parameters, and determine the geometric derivatives of the continuous mapping relationship with respect to the path parameters. It should be noted that this step establishes a continuous mapping from path parameters to joint positions and their higher-order derivatives, decoupling the originally time-dependent complex kinematic problem into a purely geometric problem, realizing the separation of path shape and time parameters, and providing a static geometric basis for subsequent time optimization; S2. Divide the entire path parameters into several continuous segments, and use the motion time interval corresponding to each segment as the variable to be optimized. Predefine the basic motion law of the path parameters changing with time for each segment. It should be noted that this step discretizes the continuous time domain into N segments, with each segment having a time interval of N. To optimize variables and pre-determine segment parameters. It is a higher-order polynomial, which makes the motion waveform change from The unique implicit determination significantly reduces the degrees of freedom for optimization while ensuring smooth motion within the segment; S3. Define pseudo-motion state variables to characterize the speed of path progress, and transform the higher-order dynamic constraints of the joint space into a set of algebraic inequalities that depend only on the pseudo-motion state variables, geometric derivatives of each order, and the motion time interval of each segment based on the chain rule. It should be noted that pseudo-motion state variables are introduced ( , The chain rule is used to express dynamic quantities such as joint velocity, acceleration, and torque as functions of geometric derivatives and pseudovariables. Then, combined with the nth-degree polynomial model from step S2, it is completely transformed into a function of... Algebraic inequalities are used to staticize, algebraize, and optimize dynamic constraints; S4. Construct and solve a nonlinear programming model with the goal of minimizing the sum of all segmented motion time intervals and a set of algebraic inequalities as constraints to obtain the optimal time interval sequence. It should be noted that a nonlinear programming model is constructed with the objective of minimizing the sum of the time intervals of all segmented motions. This completely transforms the trajectory planning problem into a parameter optimization problem, and a hybrid algorithm is used to solve it. This avoids getting trapped in local minima and ensures convergence accuracy, while efficiently searching for the time-optimal solution within the feasible region. S5. Based on the optimal time interval sequence and the basic motion law, reconstruct the continuous function of the path parameters over time, and combine the continuous mapping relationship to generate the full trajectory of the position, velocity and acceleration of the robotic arm joint space. It should be noted that, based on optimality The sequence is reconstructed into an nth-degree polynomial, and then the joint trajectory is calculated inversely using a pre-stored continuous mapping function and its derivative. The entire process does not require real-time solving of inverse kinematics or differential equations. The generated trajectory can satisfy path constraints and dynamic limitations and can be directly used for controller execution.

[0034] This embodiment achieves globally time-optimal scheduling of high-order dynamic performance under strict Cartesian path constraints, resolving the contradiction between path accuracy and dynamic performance that traditional methods struggle to balance.

[0035] Furthermore, as a refinement and extension of the specific implementation of the above embodiments, in order to fully illustrate the specific implementation process in this embodiment, another robot strict path-time optimal trajectory planning method based on arc length segmentation optimization is provided. Taking a specific six-degree-of-freedom serial industrial robotic arm as an example, the method includes the following steps: This embodiment uses the UR10e six-DOF collaborative robot as the experimental subject, with n=6 joints. ； The end effector needs to move along a straight path in Cartesian space, with the starting point of the path being... The destination is Total arc length of the path The robotic arm end effector must move strictly along this straight path and satisfy the joint dynamics limit constraints shown in Table 1 below. Table 1: Joint Dynamic Limit Constraints

[0036] S1. Based on a preset Cartesian space path, establish a continuous mapping relationship between the end-effector pose and joint position along the path parameters, and determine the geometric derivatives of the continuous mapping relationship with respect to the path parameters; the specific steps of step S1 are as follows: S11. Perform dense resampling on the preset Cartesian space path, using the path parameters. Using [variable name] as the independent variable, obtain discrete end-effector pose sequences. ,in, Let j be the position vector of the j-th discrete point. Let j be the attitude vector of the j-th discrete point; S12. Perform inverse kinematics calculations on each discrete end-effector pose to obtain the corresponding joint position sequence. , n is the number of joints in the robotic arm. Let j be the joint position vector of the j-th discrete point; S13. Employ spline interpolation or polynomial fitting methods, based on... Construct joint positions with respect to path parameters continuous mapping function ; S14. For the continuous mapping function Determine the first-order geometric derivative using analytical or numerical differentiation. Second-order geometric derivative and higher-order geometric derivatives, and stored as with respect to path parameters. The lookup table or function handle is provided for direct use in subsequent steps. For example, for a straight path, the arc length step is... Dense resampling was performed at 0.001m, resulting in a total of J=501 discrete points (including the start and end points); the joint position at each discrete point was obtained by analytical inverse kinematics. A continuous mapping function of joint position with respect to path parameters is constructed using quintic spline interpolation. And the first geometric derivative is obtained by analytical differentiation. Second-order geometric derivative and third-order geometric derivative Stored as path parameters This is a lookup table for indexing, to be used for subsequent optimization. S2. Divide the entire path parameters into several continuous segments, using the motion time interval corresponding to each segment as the variable to be optimized, and predefine the basic motion law of the path parameters changing with time for each segment; the specific steps of step S2 are as follows: S21. Pass path parameters The entire range of values ​​from the starting point to the ending point is divided into N consecutive segments, and the path parameter increments for each segment are... Given that the time interval of the i-th segment is defined... Let i be the variable to be optimized, i = 1, 2, ..., N; S22. The variation of the predefined path parameters for each segment with time t is an nth-degree polynomial:

[0037] Where n is the number of terms in the polynomial, and n≥3; These are polynomial coefficients, analytically determined by the continuity boundary conditions of position, velocity, and acceleration at the beginning and end of the k-th segment; Specifically, the boundary conditions that the polynomial must satisfy (taking a fifth-degree polynomial as an example, n=5) are shown in Table 2: Table 2: Boundary conditions satisfied by the polynomial

[0038] The above boundary conditions constitute a basis for understanding the relationship between the two boundary conditions and the boundary conditions. The system of linear equations:

[0039] in, It contains The coefficient matrix of each power; It is a constant column vector composed of boundary values; Let be the vector of coefficients to be determined; It should be noted that adjustment The entire motion process can be linearly scaled: when When shrinking, and The amplitude is synchronously and proportionally amplified throughout the entire time period; when As the amplitude increases, the amplitude decreases proportionally. This linear scaling property allows the optimizer to search for polynomial coefficients without needing to know their specific values. It can implicitly control all motion states within a segment; S23. Establish polynomial coefficients Time interval between segments of motion The implicit mapping relationship is uniquely determined by a system of linear equations constituting continuous boundary conditions; based on the implicit mapping relationship, the adjustment... This allows for proportional scaling of the magnitudes of the time derivatives of the path parameters within the corresponding segments, thus avoiding explicit calculation of polynomial coefficients during the optimization process. For example, the entire path parameter [0, 0.5] m is divided into N=50 consecutive segments, with each segment having a path increment. =0.01 m; time interval of each segment of motion The variables to be optimized are 50 in total; for each segment, a predefined fifth-degree polynomial (n=5) describes the change of path parameters over time:

[0040] The coefficients are uniquely determined by the boundary conditions for the continuity of position, velocity, and acceleration between segments, and are related to... Constructing implicit mapping relationships; regulating Scalable segment , , The amplitude does not require explicit calculation of the polynomial coefficients; S3. Define pseudo-motion state variables to represent the speed of path progression. Based on the chain rule, transform the higher-order dynamic constraints of the joint space into a system of algebraic inequalities that depend only on the pseudo-motion state variables, geometric derivatives of each order, and the motion time intervals of each segment. The specific steps of step S3 are as follows: S31. Based on path parameters Pseudo-motion state variables are defined as state variables representing the speed of path progress. These pseudo-motion state variables include pseudo-velocities. pseudo-acceleration and pseudo-accelerometer ; S32. Based on the chain rule, the joint velocity... Joint acceleration Represented as a function of geometric derivatives and pseudo-motion state variables:

[0041] And further accelerate the joints Represented as:

[0042] S33. Transform the joint velocity constraint into a constraint about... Algebraic inequalities:

[0043] in, The first geometric derivative determined in step S14, This is the joint velocity limit vector; Algebraic inequalities of joint velocity constraints in step S33 In, the first-order geometric derivative Jacobian matrix Cartesian position derivative satisfy The inequalities of the joint velocity constraints are equivalent to those expressed by the Jacobian inverse. S34 transforms the joint acceleration constraint and the joint jerk constraint into constraints related to... , , Algebraic inequalities; The algebraic inequalities for joint acceleration constraints and joint jerk constraints in step S34 are obtained through the following chain expansions: ,

[0044] ,

[0045] And , , The values ​​within the segment are represented as follows: Explicit functions; in, Pseudo-speed, It is pseudo-acceleration. It is a pseudo-jerk. The limit of the second derivative. It is the limit value of the third derivative; S35. Constrain joint torques Transforming the robot dynamics equations into... , , Algebraic inequalities:

[0046] in, For the quality matrix, For the Coriolis and centrifugal force matrices, The gravity vector This is the limit vector of joint torque; S36. Connect the pseudo-motion state variables with the time intervals of each segmented motion. Related: Based on the nth-degree polynomial in step S22, derive... , , The expression within the segment, and further rewrite all algebraic inequalities in steps S33 to S35 as depending only on And the forms of geometric derivatives of each order, to complete the algebraic dimensionality reduction mapping of higher-order dynamic constraints; For example, a pseudo velocity is defined. pseudo-acceleration pseudo-acceleration Based on the geometric derivatives pre-stored in step S1, the joint velocity, joint acceleration, and joint jerk are expressed as follows: ,

[0047] Substituting into the joint limit, we get the algebraic inequality: , ,

[0048] The standard recursive Newton-Euler method is used to calculate the dynamic model of the robotic arm. , , Transform the torque constraint into:

[0049] Based on the piecewise expression of the fifth-degree polynomial, , , Represented as Explicit functions (e.g., for a fifth-degree polynomial, we have) Its maximum value, average value, etc. within the segment can be parsed as (algebraic expressions); ultimately all constraints are rewritten to depend only on and the form of geometric derivatives of each order; S4. Construct and solve a nonlinear programming model with the goal of minimizing the sum of all segmented motion time intervals and a set of algebraic inequalities as constraints to obtain the optimal time interval sequence. The specific steps of step S4 are as follows: S41. Construct a nonlinear programming model, setting the objective function of the nonlinear programming model to the sum of the time intervals of all segmented motions:

[0050] Where N is the number of segments; S42. Add the system of algebraic inequalities obtained in step S3 as constraints to the model. The constraints include joint velocity constraints, joint acceleration constraints, joint jerk constraints, torque constraints, and pseudo-velocity nonnegativity constraints. and time positive definiteness constraints ; S43. Solving the nonlinear programming model using a hybrid iterative algorithm: Perform the global search phase, with For each individual, a genetic algorithm is applied to perform global optimization within the feasible region to obtain an approximate optimal solution as the initial seed. In the local optimization phase, using the output of the genetic algorithm as the initial value, a sequential quadratic programming algorithm is applied for a fine search, iterating until the Karush-Kuhn-Tucker path parameter conditions are met, and the optimal time interval sequence is output. ; The specific steps of step S43 are as follows: S431. Perform a global search using the genetic algorithm: The time interval of each segment of motion For individual encoding, real number encoding or binary encoding is used, and population size, crossover probability, and mutation probability are set; where N is the number of segments; The reciprocal of the objective function of the nonlinear programming model As the fitness function, the algebraic inequalities in step S42 are used as constraints. For individuals that do not meet the constraints, the fitness is corrected by introducing a penalty term that is positively correlated with the amount of constraint violation. Iteratively perform selection, crossover, and mutation operations until a preset maximum number of generations is reached or the fitness improvement is less than a set threshold. Then, output the best individual in the current population as the global approximate optimal solution. ; S432. Perform local optimization of sequential quadratic programming: The best individual output by the genetic algorithm As initial values ​​for the sequential quadratic programming algorithm; Set the convergence accuracy and maximum number of iterations; The Hessian matrix approximation of the Lagrange function is updated using a quasi-Newton method, transforming the nonlinear programming problem into a series of quadratic programming subproblems that are solved iteratively. In each iteration, based on the current Calling the pre-stored geometric derivatives from step S14 and the pseudo-motion state variables established in step S36, and The explicit relationship between the constraint violation degree and the gradient of the objective function is calculated. The iteration terminates when the Karush-Kuhn-Tucker condition is met or the iteration step size is less than the convergence accuracy, and outputs the optimal time interval sequence. ; For example, construct a nonlinear programming model: The constraints are all the algebraic inequalities after the transformation in step S3, and , ; A hybrid approach combining genetic algorithm and sequential quadratic programming is used to solve the problem. Genetic algorithm parameters: population size P=100, real-number encoding, crossover probability pc=0.8, mutation probability pm=0.1, maximum number of generations Gmax=200, fitness function is... A dynamic penalty factor λ=10 is applied to individuals who are deemed infeasible. 3 (Multiply by 1.1 per generation); by the 85th generation, the fitness improvement has been less than 10 for 20 consecutive generations. 4 Output the optimal individual as the initial value. ; Sequence quadratic programming parameters: The initial value is given, and the convergence accuracy is ε=10. 6 The maximum number of iterations is 1000. The Hessian matrix is ​​updated using the BFGS quasi-Newton method. After 23 iterations, the KKT conditions are satisfied, and the optimal time interval sequence is output. ; Optimization result: Total exercise time Each section The distribution exhibits a typical time-optimal characteristic of being slow at both ends and fast in the middle; S5. Based on the optimal time interval sequence and the basic motion law, reconstruct the continuous function of the path parameters over time, and combine the continuous mapping relationship to generate the full trajectory of the position, velocity, and acceleration of the robotic arm joint space; the specific steps of step S5 are as follows: S51. Based on the optimal time interval sequence Accumulate to generate global time nodes k=0,1,…,N, where ; S52. Based on the predefined polynomial model and optimal time interval sequence in step S22, reconstruct the continuous smooth function of the path parameters over time. Quartic spline interpolation is used to ensure It has continuous second derivatives and the interpolation error is controllable; S53. Call the geometric derivative lookup table or function stored in step S14, and combine it with the continuous mapping relationship. Inverse calculation of the entire joint position trajectory:

[0051] S54. Calculate joint velocity and joint acceleration trajectories based on the chain rule:

[0052] S55. Outputs the position, velocity, and acceleration trajectory of the robotic arm joint space throughout its entire trajectory, for controller execution or offline simulation. For example, by Accumulate to generate global time nodes The global path parameter function is reconstructed using quartic spline interpolation. ,ensure (That is, the function and its first and second derivatives are continuous); calling pre-stored... Use lookup tables and geometric derivatives to calculate joint positions. Joint speed Joint acceleration Sampling frequency 1000Hz, output all joint trajectories.

[0053] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0054] like Figure 2 As shown, the following are embodiments of the robot strict path time optimal trajectory planning system based on arc length segmentation optimization provided by this disclosure. This system and the robot strict path time optimal trajectory planning method based on arc length segmentation optimization in the above embodiments belong to the same inventive concept. For details not described in detail in the embodiments of the robot strict path time optimal trajectory planning system based on arc length segmentation optimization, please refer to the embodiments of the robot strict path time optimal trajectory planning method based on arc length segmentation optimization described above.

[0055] The system includes: The path geometric feature extraction module is used to establish a continuous mapping relationship between the end pose of the robotic arm and the joint position along the path parameters based on a preset Cartesian space path, and to determine the geometric derivatives of the continuous mapping relationship with respect to the path parameters. The time-domain discretization module is used to divide the entire path parameters into several continuous segments, with the motion time interval corresponding to each segment as the variable to be optimized, and to predefine the basic motion law of the path parameters changing with time for each segment. The algebraization module of dynamic constraints is used to define pseudo-motion state variables that characterize the speed of path progress. Based on the chain rule, it transforms the high-order dynamic constraints of the joint space into a set of algebraic inequalities that depend only on the pseudo-motion state variables, geometric derivatives of each order, and the motion time interval of each segment. The optimization solution module is used to construct and solve a nonlinear programming model with the goal of minimizing the sum of all segmented motion time intervals and a set of algebraic inequalities as constraints, to obtain the optimal time interval sequence. The trajectory reconstruction module is used to reconstruct the continuous function of path parameters over time based on the optimal time interval sequence and the basic motion law, and generate the full trajectory of position, velocity and acceleration of the robotic arm joint space by combining the continuous mapping relationship.

[0056] This embodiment utilizes the interactive collaboration of a path geometric feature extraction module, a time domain discretization module, algebraic dynamic constraint module, optimization solution module, and trajectory reconstruction module. Through path parameterization and time discretization optimization, it algebraizes high-order dynamic constraints, taking into account both path accuracy and dynamic performance, and can generate time-optimal, smooth throughout, and physically feasible joint trajectories.

[0057] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot rigorous path-time optimal trajectory planning method based on arc length piecewise optimization, characterized in that, Includes the following steps: S1. Based on a preset Cartesian space path, establish a continuous mapping relationship between the end-effector pose and joint position along the path parameters, and determine the geometric derivatives of the continuous mapping relationship with respect to the path parameters. S2. Divide the entire path parameters into several continuous segments, and use the motion time interval corresponding to each segment as the variable to be optimized. Predefine the basic motion law of the path parameters changing with time for each segment. S3. Define pseudo-motion state variables to characterize the speed of path progress, and transform the higher-order dynamic constraints of the joint space into a set of algebraic inequalities that depend only on the pseudo-motion state variables, geometric derivatives of each order, and the motion time interval of each segment based on the chain rule. S4. Construct and solve a nonlinear programming model with the goal of minimizing the sum of all segmented motion time intervals and a set of algebraic inequalities as constraints to obtain the optimal time interval sequence. S5. Based on the optimal time interval sequence and the basic motion law, reconstruct the continuous function of the path parameters over time, and combine the continuous mapping relationship to generate the full trajectory of the position, velocity and acceleration of the robotic arm joint space.

2. The robot strictly path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 1, characterized in that, The specific steps of step S1 are as follows: S11. Perform dense resampling on the preset Cartesian space path, using the path parameters. Using [variable name] as the independent variable, obtain discrete end-effector pose sequences. ,in, Let j be the position vector of the j-th discrete point. Let j be the attitude vector of the j-th discrete point; S12. Perform inverse kinematics calculations on each discrete end-effector pose to obtain the corresponding joint position sequence. , n is the number of joints in the robotic arm. Let j be the joint position vector of the j-th discrete point; S13. Employ spline interpolation or polynomial fitting methods, based on... Construct joint positions with respect to path parameters continuous mapping function ; S14. For the continuous mapping function Determine the first-order geometric derivative using analytical or numerical differentiation. Second-order geometric derivative and higher-order geometric derivatives, and stored as with respect to path parameters. The lookup table or function handle is provided for direct use in subsequent steps.

3. The robot rigorous path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 2, characterized in that, The specific steps of step S2 are as follows: S21. Pass path parameters The entire range of values ​​from the starting point to the ending point is divided into N consecutive segments, and the path parameter increments for each segment are... Given that the time interval of the i-th segment is defined... Let i be the variable to be optimized, i = 1, 2, ..., N; S22. The variation of the predefined path parameters for each segment with time t is an nth-degree polynomial: Where n is the number of terms in the polynomial, and n≥3; These are polynomial coefficients, analytically determined by the continuity boundary conditions of position, velocity, and acceleration at the beginning and end of the k-th segment; S23. Establish polynomial coefficients Time interval between segments of motion The implicit mapping relationship is uniquely determined by a system of linear equations constituting continuous boundary conditions; based on the implicit mapping relationship, the adjustment... This allows for proportional scaling of the magnitudes of the time derivatives of the path parameters within the corresponding segments, thus avoiding explicit calculation of polynomial coefficients during the optimization process.

4. The robot rigorous path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 3, characterized in that, The specific steps of step S3 are as follows: S31. Based on path parameters Pseudo-motion state variables are defined as state variables representing the speed of path progress. These pseudo-motion state variables include pseudo-velocities. pseudo-acceleration and pseudo-accelerometer ; S32. Based on the chain rule, the joint velocity... Joint acceleration Represented as a function of geometric derivatives and pseudo-motion state variables: And further accelerate the joints Represented as: S33. Transform the joint velocity constraint into a constraint about... Algebraic inequalities: in, The first geometric derivative determined in step S14, This is the joint velocity limit vector; S34 transforms the joint acceleration constraint and the joint jerk constraint into constraints related to... , , Algebraic inequalities; S35. Constrain joint torques Transforming the robot dynamics equations into... , , Algebraic inequalities: in, For the quality matrix, For the Coriolis and centrifugal force matrices, The gravity vector This is the limit vector of joint torque; S36. Connect the pseudo-motion state variables with the time intervals of each segmented motion. Related: Based on the nth-degree polynomial in step S22, derive... , , The expression within the segment, and further rewrite all algebraic inequalities in steps S33 to S35 as depending only on And the forms of geometric derivatives of each order, to complete the algebraic dimensionality reduction mapping of higher-order dynamic constraints.

5. The robot rigorous path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 4, characterized in that, Algebraic inequalities of joint velocity constraints in step S33 In, the first-order geometric derivative Jacobian matrix Cartesian position derivative satisfy The inequality of the joint velocity constraint is equivalent to the form expressed by the Jacobi inverse.

6. The robot strictly path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 4, characterized in that, The algebraic inequalities for joint acceleration constraints and joint jerk constraints in step S34 are obtained through the following chain expansions: , , And , , The values ​​within the segment are represented as follows: Explicit functions; in, Pseudo-speed, It is pseudo-acceleration. It is a pseudo-jerk. The limit of the second derivative. It is the limit value of the third derivative.

7. The robot rigorous path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 4, characterized in that, The specific steps of step S4 are as follows: S41. Construct a nonlinear programming model, setting the objective function of the nonlinear programming model to the sum of the time intervals of all segmented motions: Where N is the number of segments; S42. Add the system of algebraic inequalities obtained in step S3 as constraints to the model. The constraints include joint velocity constraints, joint acceleration constraints, joint jerk constraints, torque constraints, and pseudo-velocity nonnegativity constraints. and time positive definiteness constraints ; S43. Solving the nonlinear programming model using a hybrid iterative algorithm: Perform the global search phase, with For each individual, a genetic algorithm is applied to perform global optimization within the feasible region to obtain an approximate optimal solution as the initial seed. In the local optimization phase, using the output of the genetic algorithm as the initial value, a sequential quadratic programming algorithm is applied for a fine search, iterating until the Karush-Kuhn-Tucker path parameter conditions are met, and the optimal time interval sequence is output. .

8. The robot strictly path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 7, characterized in that, The specific steps of step S43 are as follows: S431. Global search phase of genetic algorithm: The time interval of each segment of motion For individual encoding, real number encoding or binary encoding is used, and population size, crossover probability, and mutation probability are set; where N is the number of segments; The reciprocal of the objective function of the nonlinear programming model As the fitness function, the algebraic inequalities in step S42 are used as constraints. For individuals that do not meet the constraints, the fitness is corrected by introducing a penalty term that is positively correlated with the amount of constraint violation. Iteratively perform selection, crossover, and mutation operations until a preset maximum number of generations is reached or the fitness improvement is less than a set threshold. Then, output the best individual in the current population as the global approximate optimal solution. ; S432. Local optimization stage of sequential quadratic programming: The best individual output by the genetic algorithm As initial values ​​for the sequential quadratic programming algorithm; Set the convergence accuracy and maximum number of iterations; The Hessian matrix approximation of the Lagrange function is updated using a quasi-Newton method, transforming the nonlinear programming problem into a series of quadratic programming subproblems that are solved iteratively. In each iteration, based on the current Calling the pre-stored geometric derivatives from step S14 and the pseudo-motion state variables established in step S36, and The explicit relationship between the constraint violation degree and the gradient of the objective function is calculated. The iteration terminates when the Karush-Kuhn-Tucker condition is met or the iteration step size is less than the convergence accuracy, and outputs the optimal time interval sequence. .

9. The robot rigorous path-time optimal trajectory planning method based on arc length segmentation optimization according to claim 7, characterized in that, The specific steps of step S5 are as follows: S51. Based on the optimal time interval sequence Accumulate to generate global time nodes k=0,1,…,N, where ; S52. Based on the predefined polynomial model and optimal time interval sequence in step S22, reconstruct the continuous smooth function of the path parameters over time. Quartic spline interpolation is used to ensure It has continuous second derivatives and the interpolation error is controllable; S53. Call the geometric derivative lookup table or function stored in step S14, and combine it with the continuous mapping relationship. Inverse calculation of the entire joint position trajectory: S54. Calculate joint velocity and joint acceleration trajectories based on the chain rule: S55. Outputs the position, velocity, and acceleration trajectory of the robotic arm joints throughout the entire process, for controller execution or offline simulation.

10. A robot rigorous path-time optimal trajectory planning system based on arc length piecewise optimization, characterized in that, include: The path geometric feature extraction module is used to establish a continuous mapping relationship between the end pose of the robotic arm and the joint position along the path parameters based on a preset Cartesian space path, and to determine the geometric derivatives of the continuous mapping relationship with respect to the path parameters. The time-domain discretization module is used to divide the entire path parameters into several continuous segments, with the motion time interval corresponding to each segment as the variable to be optimized, and to predefine the basic motion law of the path parameters changing with time for each segment. The algebraization module of dynamic constraints is used to define pseudo-motion state variables that characterize the speed of path progress. Based on the chain rule, it transforms the high-order dynamic constraints of the joint space into a set of algebraic inequalities that depend only on the pseudo-motion state variables, geometric derivatives of each order, and the motion time interval of each segment. The optimization solution module is used to construct and solve a nonlinear programming model with the goal of minimizing the sum of all segmented motion time intervals and a set of algebraic inequalities as constraints, to obtain the optimal time interval sequence. The trajectory reconstruction module is used to reconstruct the continuous function of path parameters over time based on the optimal time interval sequence and the basic motion law, and generate the full trajectory of position, velocity and acceleration of the robotic arm joint space by combining the continuous mapping relationship.

Images