A robot time-optimal trajectory planning method and system based on second-order cone programming
By using a second-order cone programming method, the robot trajectory planning problem is transformed into a standard convex optimization form, which solves the problems of global optimality and computational efficiency under third-order constraints, achieves smooth time-optimal trajectory planning, and reduces the impact and vibration of the robotic arm.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- HUAZHONG UNIV OF SCI & TECH
- Filing Date
- 2026-04-16
- Publication Date
- 2026-07-21
AI Technical Summary
Existing convex optimization methods struggle to guarantee global optimality and computational efficiency when dealing with robot trajectory planning involving third-order constraints, leading to trajectory planning failures or exponential growth in solution size.
A method based on second-order cone programming is adopted. By introducing pseudo-velocity variables and relaxation inequalities, the non-convex trajectory planning problem is transformed into a standard convex optimization second-order cone programming model. The second-order cone programming model is then used to solve the robot's time-optimal trajectory.
It achieves globally optimal time-optimal trajectory planning, eliminates acceleration step phenomena, improves trajectory smoothness, reduces the impact and vibration of the robotic arm during processing, and enhances the motion stability of the system.
Smart Images
Figure CN122425669A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the technical field of industrial robot motion control, and more specifically, relates to a robot time-optimal trajectory planning method and system based on second-order cone programming. Background Technology
[0002] Time-Optimal Trajectory Planning (TOTP) is a core technology for improving the productivity of automated systems. In tens of thousands of reciprocating operations, even small time reductions can accumulate into significant increases in output and reductions in production costs, resulting in substantial economic benefits. The essence of TOTP is to find a motion strategy that minimizes the total execution time while satisfying the robot's kinematics and its physical limits. Due to the strong coupling and nonlinear characteristics of robot kinematics and its equations, path parameterization techniques are typically used to map high-dimensional motion to a low-dimensional phase plane for solution. Currently, robot trajectory planning methods can be mainly divided into numerical integration-based methods and convex optimization-based methods.
[0003] The numerical integration method is based on the "bang-bang" control principle. Its core idea is to determine the maximum velocity constraint curve in the phase plane and perform bidirectional integration along the boundaries of maximum acceleration and maximum deceleration. Theoretically, this method can obtain the absolutely time-optimal trajectory. However, due to the discontinuous jumps in acceleration at the switching points, it is easy to induce vibrations in the mechanical system, causing significant impact on the actuator.
[0004] Convex optimization-based methods transform non-convex problems with nonlinear and higher-order kinematic constraints into standard convex optimization forms through nonlinear variable substitution. Leveraging the property that local optima in convex optimization problems are equivalent to global optima, time-optimal trajectories can be directly obtained. Compared to numerical integration methods, convex optimization methods exhibit extremely high numerical robustness and solution success rates, making them the mainstream and efficient methods in the field of offline trajectory optimization.
[0005] While existing convex optimization methods perform well in handling second-order constraints, a key technical bottleneck remains how to achieve reasonable convexification for complex trajectory planning involving third-order constraints (such as jerk) to obtain a standard convex optimization problem. If the discretization step size is too large, it cannot accurately capture the constraint boundaries; if the step size is too small, the solution size will grow exponentially.
[0006] Therefore, how to construct an optimization solution framework that can guarantee global optimality while taking into account computational efficiency for time-optimal trajectory planning problems with third-order jerk constraints is a key problem that urgently needs to be solved in current robot trajectory planning technology. Summary of the Invention
[0007] To address the aforementioned deficiencies or improvement needs of existing technologies, this invention provides a robot time-optimal trajectory planning method and system based on second-order cone programming, solving the problem that convex processing under third-order constraints in robot trajectory planning research cannot guarantee global optimality and balance computational efficiency.
[0008] To achieve the above objectives, according to one aspect of the present invention, a robot time-optimal trajectory planning method based on second-order cone programming is provided, the planning method comprising the following steps: S1 For the robot's preset motion path, construct a non-convex trajectory planning optimization model with the shortest motion time as the objective function; S2 constructs pseudo-velocity variables and discretizes the motion path, converting angular velocity and angular acceleration into linear relationships only related to pseudo-velocities, transforming the objective function into a summation relationship, and converting angular jerk into a nonlinear algebraic constraint on the pseudo-velocity variables; introduces auxiliary variables and establishes relaxation inequalities, transforming the objective function into a linear weighted sum of auxiliary variables; using the transformed angular acceleration, angular jerk, and objective function, the non-convex trajectory planning optimization model is transformed into a standard convex optimization second-order cone programming model; S3 solves the standard convex optimization second-order cone programming model to obtain the optimal robot joint angular velocity, angular acceleration, and angular jerk.
[0009] More preferably, in step S1, the non-convex trajectory planning optimization model is as follows:
[0010] in, These are the robot's angular velocity, angular acceleration, and angular jerk, respectively. Representing the first The limits of joint velocity, acceleration, and jerk, where T is the processing time for a given path. This is a time parameter.
[0011] More preferably, in step S2, the angular velocity and angular acceleration are converted into linear relationships that are only related to pseudo-velocities, as follows:
[0012]
[0013] Where k is the number of the discrete point and discrete interval, and i is the joint number. Let be the angular velocity and angular acceleration of the i-th joint at the k-th discrete point, respectively. , , For constant terms, For path parameters, Let be the length of the k-th discrete interval of the path parameter s. Let be the first and second derivatives of the i-th joint position at the k-th discrete point with respect to the path parameter s, respectively. The pseudo-velocity variable introduced here represents the square of the robot's end-effector velocity at the k-th discrete point.
[0014] More preferably, in step S2, the objective function transformation summation relation is as follows:
[0015] Where T is the total processing time of the path, N is the number of discrete intervals, and k is the index of the discrete point and the discrete interval. The length of the k-th discrete interval of the path parameters. Let be the pseudo-velocity variable at the kth discrete point.
[0016] More preferably, in step S2, the nonlinear algebraic constraint of the angular jerk with respect to the pseudo-velocity variable is as follows:
[0017] Where k is the number of the discrete point and discrete interval, and i is the joint number. This represents the jerk of the i-th joint at the k-th discrete point. For optimization variables Nonlinearly related terms, Let be the length of the k-th discrete interval of the path parameter s. It is a pseudo-velocity variable. , , For constant terms, These are the first and second derivatives of the i-th joint position at the k-th discrete point with respect to the path parameter s, respectively.
[0018] More preferably, in step S2, the relaxation inequality relationship is as follows:
[0019]
[0020] Where k is the number of the discrete point and the discrete interval. This represents the pseudo-velocity variable at the k-th discrete point. , This represents the auxiliary variable at the k-th discrete point.
[0021] More preferably, in step S2, the objective function is converted into a linearly weighted formula with respect to the auxiliary variables as follows:
[0022] Where T represents the total processing time of the path. The length of the k-th discrete interval of the path parameters. These are the auxiliary variables introduced.
[0023] More preferably, in step S2, the standard convex optimization second-order cone programming model is as follows:
[0024] Where k is the number of the discrete point and the discrete interval. Indicates the problem to be solved Variable sequence, This represents the pseudo-velocity variable at the k-th discrete point. , Let N represent the auxiliary variable at the k-th discrete point, and N be the number of discrete intervals. This represents the length of the k-th discrete interval of the path parameter s. , , , , , All are constant terms. Let represent the first and second derivatives of the i-th joint position at the k-th discrete point with respect to the path parameter s, respectively. , , These are the limit values of the velocity, acceleration, and jerk of the i-th joint, respectively.
[0025] More preferably, in step S3, the standard convex optimization second-order cone programming model is solved using SeDuMi, SDPT3, or CVX solvers.
[0026] According to another aspect of the present invention, a robot time-optimal trajectory planning system based on second-order cone programming is provided, the system including an actuator for executing the robot time-optimal trajectory planning method based on second-order cone programming described above.
[0027] In summary, the technical solutions conceived by this invention have the following beneficial effects compared with the prior art: 1. This invention transforms the originally highly nonlinear and non-convex time-optimal trajectory planning problem into a standard convex optimization form by introducing pseudo-velocity variables and relaxation inequalities. Convex optimization problems have the characteristics of local and global optimal solutions, thus enabling the stable acquisition of the global optimal running time without relying on complex parameter adjustments or random initial value guesses, effectively avoiding the planning failure caused by traditional methods getting trapped in local minima.
[0028] 2. By incorporating jerk constraints into the constraints of the optimization problem, this invention achieves continuous change of the acceleration curve, effectively eliminating the acceleration step phenomenon at the speed switching point in the traditional TOTP algorithm, thereby significantly improving the high-order smoothness of the trajectory. This smooth kinematic characteristic can significantly reduce the impact and vibration generated by the robotic arm during processing and enhance the motion stability of the system under high-speed operating conditions. Attached Figure Description
[0029] Figure 1 This is a flowchart of a robot time-optimal trajectory planning method based on second-order cone programming constructed according to a preferred embodiment of the present invention. Detailed Implementation
[0030] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.
[0031] like Figure 1 The diagram shows a robot trajectory planning process based on second-order cone programming. The specific implementation is as follows: S1 Establish the joint space kinematics model of the industrial robot and the initial optimization objective.
[0032] The motion state of the robotic arm can be achieved using joint position vectors. And its derivatives. Given a geometric path, with the objective function of minimizing motion time, and based on the actual physical constraints of the robot joints, an initial problem model for time-optimal trajectory planning can be established, including constraints on joint angular velocity, angular acceleration, and angular jerk (Jerk).
[0033] in, This represents the first, second, and third derivatives of each joint of the robot with respect to time. Representing the first The limits of joint velocity, acceleration, and jerk.
[0034] S2 Parameter Space Mapping and Pseudo-Velocity Variable Introduction: (1) Convert joint velocities and accelerations into linear expressions of robot end-effector velocities and accelerations. In joint space, the position and path of each joint The functional relationship between them can be expressed as: ,in For path parameters, , This represents the number of joints in the robotic arm.
[0035] According to the chain rule, the derivatives of joint variables with respect to time can be expanded as follows with respect to path parameters. The derivative and Combination of derivatives with respect to time. Joint position. Taking the first and second derivatives with respect to time yields the joint velocity-acceleration, which can be expressed in the parameter domain as:
[0036] in Indicates the path parameters of joint position The first and second derivatives can be regarded as constant terms in this invention.
[0037] (2) Introduce pseudo-velocity variables to convert joint velocity, acceleration and objective function into linear expressions of pseudo-velocity and derivative.
[0038] Introducing pseudo-velocity variables ,right Regarding the path Taking the derivative, we get
[0039] Furthermore, the joint velocity can be expressed as:
[0040] in This is a constant term. Joint acceleration can be expressed as:
[0041] For the trajectory planning problem, the objective function is the motion time. It can be determined by pseudo velocity. Represented as:
[0042] (3) Discretize the path parameters and convert the angular acceleration and objective function into a pseudo-velocity relationship.
[0043] Path parameters Discretized Each small interval corresponds to a discrete point sequence .make Corresponding to the Each path parameter value, It's a long walk away. For the first Pseudo-velocities at discrete points This represents the pseudo-acceleration at this discrete point. To improve the accuracy of the velocity curve, the discrete interval... It should be as small as possible.
[0044] Using the finite difference method to identify pseudo-acceleration Approximately expressed as The angular acceleration of a robot's joints can be expressed as:
[0045] in This is a constant term.
[0046] Using the trapezoidal rule, the integral objective function is transformed into an algebraic summation form: .
[0047] (4) Construct a discretized third-order constraint model based on acceleration difference: Record No. The acceleration over each interval is The time interval of the movement is To effectively assess the jerk changes between adjacent trajectory segments, the central difference method is used to estimate the jerk. For the jerk changes caused by the first... Section and the The average jerk of adjacent intervals composed of segments It can be defined as the average of the rate of change of acceleration over time between adjacent segments:
[0048] By combining the mapping relationship between pseudo-velocities and joint accelerations, the third-order jerk constraint can be characterized as a pseudo-velocity variable about three consecutive discrete points. Nonlinear algebraic constraints:
[0049] in: For optimization variables Nonlinearly related terms; This is a constant term.
[0050] (5) Introduce auxiliary variables to achieve linearization of the objective function and constraints: Analyze the reconstructed optimization model and determine the square root terms of the variables included in the objective function and the denominator of the third-order constraint. The nonlinear combinations of these factors (such as fractional structures) are the main reason for the nonconvexity of the problem. Such nonconvex models are difficult to optimize using conventional numerical optimization methods, which often fail to guarantee global optimality and computational efficiency.
[0051] Introducing two sets of core auxiliary variables and And establish relaxation inequality relations: as well as Based on this, the original objective function is reconstructed as a function of auxiliary variables. linear weighted sum form .
[0052] (6) Perform equivalent reconstruction using second-order cone programming (SOCP): By utilizing the geometric properties of a rotated second-order cone, the inequalities of the above auxiliary variables are transformed into the standard second-order cone constraint form:
[0053]
[0054] Through this equivalent reconstruction, the original non-convex trajectory planning problem is transformed into a standard convex optimization second-order cone programming problem. The entire trajectory planning problem model is shown below:
[0055] S3 constructs a modular solution framework and performs optimizations.
[0056] Define decision variables using optimization modeling tools (such as YALMIP). The system constructs sets of second-order cone constraints and various boundary constraints. It calls mature solvers based on the interior point method framework (such as SeDuMi) for iterative calculations, and returns the globally optimal pseudo-velocity sequence after meeting the preset accuracy tolerance, thereby obtaining the time-optimal trajectory.
[0057] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
Claims
1. A time-optimal trajectory planning method for robots based on second-order cone programming, characterized in that, The planning methodology includes the following steps: S1 For the robot's preset motion path, construct a non-convex trajectory planning optimization model with the shortest motion time as the objective function; S2 constructs pseudo-velocity variables and discretizes the motion path, converting angular velocity and angular acceleration into linear relationships only related to pseudo-velocities, transforming the objective function into a summation relationship, and converting angular jerk into a nonlinear algebraic constraint on the pseudo-velocity variables; introduces auxiliary variables and establishes relaxation inequalities, transforming the objective function into a linear weighted sum of auxiliary variables; using the transformed angular acceleration, angular jerk, and objective function, the non-convex trajectory planning optimization model is transformed into a standard convex optimization second-order cone programming model; S3 solves the standard convex optimization second-order cone programming model to obtain the optimal robot joint angular velocity, angular acceleration, and angular jerk.
2. The robot time-optimal trajectory planning method based on second-order cone programming as described in claim 1, characterized in that, In step S1, the non-convex trajectory planning optimization model is as follows: in, Let be the angular velocity, angular acceleration, and angular jerk of the i-th joint of the robot, respectively. Representing the first The limits of joint velocity, acceleration, and jerk, where T is the processing time for a given path. This is a time parameter.
3. A robot time-optimal trajectory planning method based on second-order cone programming as described in claim 1 or 2, characterized in that, In step S2, the angular velocity and angular acceleration are converted into linear relationships that are only related to pseudo-velocities, as follows: Where k is the number of the discrete point and discrete interval, and i is the joint number. Let be the angular velocity and angular acceleration of the i-th joint at the k-th discrete point, respectively. , , For constant terms, For path parameters, Let be the length of the k-th discrete interval of the path parameter s. Let be the first and second derivatives of the i-th joint position at the k-th discrete point with respect to the path parameter s, respectively. The pseudo-velocity variable introduced here represents the square of the robot's end-effector velocity at the k-th discrete point.
4. A robot time-optimal trajectory planning method based on second-order cone programming as described in claim 1 or 2, characterized in that, In step S2, the objective function is transformed into the following summation relation: Where T is the total processing time of the path, N is the number of discrete intervals, and k is the index of the discrete point and the discrete interval. The length of the k-th discrete interval of the path parameters. Let be the pseudo-velocity variable at the kth discrete point.
5. A robot time-optimal trajectory planning method based on second-order cone programming as described in claim 1 or 2, characterized in that, In step S2, the nonlinear algebraic constraint of the angular acceleration with respect to the pseudo-velocity variable is as follows: Where k is the number of the discrete point and discrete interval, and i is the joint number. This represents the angular acceleration of the i-th joint at the k-th discrete point. For optimization variables Nonlinearly related terms, Let be the length of the k-th discrete interval of the path parameter s. It is a pseudo-velocity variable. , , For constant terms, These are the first and second derivatives of the i-th joint position at the k-th discrete point with respect to the path parameter s, respectively.
6. A robot time-optimal trajectory planning method based on second-order cone programming as described in claim 1 or 2, characterized in that, In step S2, the relaxation inequality relationship is as follows: Where k is the number of the discrete point and the discrete interval. This represents the pseudo-velocity variable at the k-th discrete point. , This represents the auxiliary variable at the k-th discrete point.
7. The robot time-optimal trajectory planning method based on second-order cone programming as described in claim 6, characterized in that, In step S2, the objective function is transformed into a linearly weighted formula with respect to the auxiliary variables as follows: Where T represents the total processing time of the path. The length of the k-th discrete interval of the path parameters. These are auxiliary variables introduced.
8. The robot time-optimal trajectory planning method based on second-order cone programming as described in claim 7, characterized in that, In step S2, the standard convex optimization second-order cone programming model is as follows: Where k is the number of the discrete point and the discrete interval. Indicates the problem to be solved Variable sequence, This represents the pseudo-velocity variable at the k-th discrete point. , Let N represent the auxiliary variable at the k-th discrete point, and N be the number of discrete intervals. This represents the length of the k-th discrete interval of the path parameter s. , , , , , All are constant terms. Let represent the first and second derivatives of the i-th joint position at the k-th discrete point with respect to the path parameter s, respectively. , , These are the limit values of the velocity, acceleration, and jerk of the i-th joint, respectively.
9. A robot time-optimal trajectory planning method based on second-order cone programming as described in claim 1 or 8, characterized in that, In step S3, the standard convex optimization second-order cone programming model is solved using SeDuMi, SDPT3, or CVX solvers.
10. A time-optimal trajectory planning system for robots based on second-order cone programming, characterized in that, The system includes an actuator for performing a robot time-optimal trajectory planning method based on second-order cone programming as described in any one of claims 1-9.