A method for trajectory planning of a step-by-step, multi-stage point-alignment robotic arm along a specific path
By using adaptive mesh refinement iterative solution and a multi-stage optimal control model, the accuracy and efficiency problems caused by improper discrete node setting in the multi-stage collocation method are solved, realizing efficient and stable solution for robotic arm trajectory planning, which is applicable to industrial scenarios such as laser cutting, spraying, and welding.
Patent Information
- Application Number
- CN202611509547.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-09-28
- Publication Date
- 2026-11-17
AI Technical Summary
Existing multi-stage direct point matching methods in robotic arm trajectory planning struggle to achieve the optimal balance between fitting accuracy and computational efficiency. Manually setting discrete nodes leads to problems such as large path errors or low computational efficiency.
An adaptive grid-based iterative solution method is adopted. By constructing a geometric path discrete error minimization optimization problem with the arc length of the robotic arm end path as the independent variable, key discrete nodes are automatically determined. Based on this, a multi-stage optimal control mathematical model is constructed, and the time-optimal or energy-optimal trajectory is output.
While ensuring the accuracy of the robotic arm's end effector moving along a specified path, the computational overhead of trajectory solving is reduced, and the stability and convergence of the solution process are improved. It is particularly suitable for high-precision industrial scenarios such as laser cutting, spraying, and welding.
Smart Images

Figure CN123103669A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot motion planning and control technology, and more specifically, to a method for trajectory planning of a step-by-step, multi-stage point-alignment robotic arm along a specific path. Background Technology
[0002] In high-speed, high-precision industrial applications such as laser cutting, precision assembly, spraying, and welding, the end effector of a six-axis tandem robotic arm must strictly follow a specific geometric path (such as the workpiece contour or spraying trajectory) to perform motion tasks, while simultaneously meeting performance targets such as optimal time and lowest energy consumption. Multiphase Direct Collocation (MPDC) is a commonly used trajectory planning method. It discretizes the complete geometric path into several consecutive sub-stages, independently establishing dynamic constraint equations within each sub-stage, and achieving smooth transitions between state and control variables through boundary conditions at discrete path points between adjacent stages. This method can flexibly adapt to diverse optimization objectives such as optimal time and optimal energy.
[0003] However, existing multi-stage direct collocation methods still have inherent limitations in practical applications. When the discrete path points are sparsely set, the fitting accuracy of the geometric path is insufficient, easily leading to a large error between the actual tracking trajectory of the robotic arm and the ideal path. Conversely, when the discrete path points are densely set, although the fitting accuracy can be improved, the scale of decision variables and constraints expands dramatically, making the Jacobian matrix of the nonlinear programming problem too dense, significantly reducing solution efficiency, and even causing memory overflow. Therefore, the discrete granularity of the geometric path directly determines the trade-off between planning accuracy and computational efficiency. The existing methods' heuristic setting of the number of discrete stages lacks adaptability and makes it difficult to achieve the optimal trade-off between accuracy and efficiency under different working conditions.
[0004] Therefore, there is an urgent need to design a systematic trajectory planning method that can automatically determine the optimal discrete granularity. Summary of the Invention
[0005] This invention addresses the inherent shortcomings of existing multi-stage point allocation methods, which rely on manual experience to set discrete nodes and struggle to balance fitting accuracy and computational efficiency. It provides a step-by-step multi-stage point allocation robotic arm trajectory planning method along a specific path. Through an adaptive discrete node selection strategy, it achieves the optimal balance between fitting accuracy and computational efficiency without the need for manual pre-setting.
[0006] According to a first aspect of the present invention, a method for trajectory planning along a specific path by a step-by-step, multi-stage point-finding robotic arm is provided, comprising: S1, construct a geometric path discrete error minimization optimization problem with the arc length of a specific path at the end of the robotic arm as the independent variable, and solve it iteratively through adaptive mesh encryption to obtain key discrete nodes that meet the preset discrete error threshold. The key discrete nodes correspond to key positions on the specific path. S2, using the key discrete nodes as the stage boundary conditions of the multi-stage optimal control problem, construct and solve the multi-stage optimal control mathematical model with dynamic constraints, and output the time-optimal or energy-optimal trajectory along the specific path.
[0007] Based on the above technical solution, the present invention can also be improved as follows.
[0008] Optionally, step S1 includes: S101, taking the arc length of a specific path at the end of the robotic arm as the independent variable, introduces joint space state variables and control variables, sets the performance index as the error integral between the approximate path and the specific path in Cartesian space, and sets differential state constraints, path constraints, boundary conditions and inequality constraints to construct the optimization problem of minimizing the discrete error of the geometric path of the robotic arm; wherein, the specific path is the workpiece contour or predetermined trajectory that the end of the robotic arm needs to follow during the operation. S102, uniformly set the initial set of discrete nodes according to the arc length, and define the joint space function of the specific path; S103, Using the numerical integration method, the continuous geometric path discrete error minimization optimization problem is discretized into a nonlinear programming problem according to the discrete node set and solved to obtain the approximate path under the current discrete node; S104. Estimate the local discrete path error for each arc length interval. If the local discrete path error is greater than the preset threshold, calculate the number of new discrete nodes to be added based on the error size and insert the new nodes in the arc length interval. After updating the discrete node set, return to step S103 to solve again until the local discrete path error of all arc length intervals is not greater than the preset threshold. S105 outputs the set of key discrete nodes that meet the error requirements.
[0009] Optionally, in step S101, the minimization performance index of the geometric path discrete error minimization optimization problem is described as follows: Minimize the performance metric, expressed as:
[0010]
[0011] The combined state variables are represented as follows:
[0012] Control variables are represented as:
[0013] Differential state constraints are expressed as:
[0014] Path constraints:
[0015] Boundary conditions:
[0016]
[0017] Inequality constraints:
[0018]
[0019]
[0020] in, Joint space coordinates The Cartesian space approximation path obtained through positive kinematic mapping. For a specific path, Let the arc length be , , The initial arc length, The total arc length of the target path; Indicates linear velocity. Indicates the maximum linear velocity. Indicates linear acceleration. Indicates the maximum linear acceleration; Represents the combined state variables. Represents the differential of the combined state variables. Represents the comprehensive differential state function. Represents the forward kinematics function of the robotic arm. Represents the positive dynamics function of the robotic arm; Indicates joint angle, Indicates joint angular velocity, Represents joint angular acceleration. and These represent the initial and final states of the robotic arm joint angles, respectively. and These represent the initial and final states of the angular velocity of the robotic arm joints, respectively. and These represent the maximum angle and maximum angular velocity that the robotic arm joints can execute, respectively. The torque of the joint motor, This indicates the maximum torque that the joint motor can provide.
[0021] Optionally, in step S103, the performance index of the discretized nonlinear programming problem is expressed as:
[0022] The differential state constraint is:
[0023]
[0024] The total number of discrete collocation points is , This indicates the point number starting from 0. ; Represents constant coefficients in numerical integration methods; Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The joint space coordinates corresponding to each node. Joint space coordinates The Cartesian space approximation path obtained by positive kinematic mapping; Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The arc length of a specific path corresponding to each node. For local intervals The numerical integration method requires further discretization. The th secondary discrete point The specific path corresponding to each node; Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The values of the integrated differential state functions corresponding to each node. For local intervals Based on the requirement of discrete generation by numerical integration method The th secondary discrete point The comprehensive state variables corresponding to each node For local intervals Based on the requirement of discrete generation by numerical integration method The th secondary discrete point Control variables corresponding to each node; Indicates matching points The overall state variables at the location, Indicates matching points The overall state variables at the location.
[0025] Optionally, in step S104, the method for estimating the local discrete path error is as follows: Using the same numerical integration method as in step S103, calculate the integral of the deviation between the approximate path and the specific path in joint space over each arc length interval.
[0026] Optionally, in step S104, the formula for calculating the number of newly added discrete nodes is:
[0027] in, This represents the local error within the current interval. To preset the error threshold, Let be the order of the numerical integration method used. Rounded up, the result is the number of new nodes.
[0028] Optionally, step S2 includes: S201, using each node in the set of key discrete nodes as the start and end states of each stage, construct the boundary conditions and continuity conditions between stages to ensure that the position, velocity and time of adjacent stages are continuous at the connection point. S202, Based on the inter-stage boundary conditions, construct a complete multi-stage optimal control mathematical model, which includes performance indicators, differential constraints, and inequality constraints; S203, the optimal control mathematical model is discretized into a nonlinear programming mathematical model using the multi-stage direct collocation method and solved to output the time-optimal or energy-optimal trajectory along the specific path.
[0029] Optionally, in step S201, the inter-stage continuity conditions include: The position, velocity, and time are continuous at the connection point between adjacent stages.
[0030] Optionally, in step S202, the performance indicators of the multi-stage optimal control mathematical model include:
[0031] in, For the number of stages, Let i be the boundary index function for stage i. Let i be the process index function for stage i. and These are the start and end times of stage i, respectively. Let i be the state variable of stage i. For stage i, and These are the initial state vector and the final state vector of stage i, respectively. and As weight.
[0032] According to a second aspect of the present invention, a trajectory planning system for a step-by-step, multi-stage point-finding robotic arm along a specific path is provided, comprising: The path node planning module is used to construct a geometric path discrete error minimization optimization problem with the arc length of a specific path at the end of the robotic arm as the independent variable. It solves the problem through adaptive mesh densification and iteration to obtain key discrete nodes that meet the preset discrete error threshold. The key discrete nodes correspond to key positions on the specific path. The optimal trajectory planning module is used to construct and solve a multi-stage optimal control mathematical model with dynamic constraints, using the key discrete nodes as stage boundary conditions for the multi-stage optimal control problem, and output the time-optimal or energy-optimal trajectory along the specific path.
[0033] According to a third aspect of the present invention, an electronic device is provided, including a memory and a processor, wherein the processor is configured to execute a computer management program stored in the memory to implement the steps of the above-described step-by-step multi-stage point-locating robotic arm trajectory planning method along a specific path.
[0034] According to a fourth aspect of the present invention, a computer-readable storage medium is provided, on which a computer management program is stored, wherein when executed by a processor, the computer management program implements the steps of the above-described step-by-step multi-stage point-locating robotic arm trajectory planning method along a specific path.
[0035] This invention provides a step-by-step, multi-stage trajectory planning method, system, electronic device, and storage medium for a robotic arm along a specific path. By decoupling path geometric accuracy assurance and dynamic optimal solution into two independent optimization stages, it eliminates the problems caused by improper selection of discrete granularity in traditional methods. In the first stage, an adaptive mesh refinement mechanism automatically determines the number and distribution of key discrete nodes based on the geometric complexity of the path. It automatically refines the mesh in regions with drastic curvature changes to ensure fitting accuracy and automatically thins it out in flat regions to reduce redundant nodes, thus achieving the preset path error requirement with the fewest possible nodes. This adaptive characteristic allows the subsequent second-stage multi-stage optimal control problem to be solved on a precise and compact stage division basis, avoiding both path deviation caused by too few nodes and decision variable inflation and a sharp drop in solution efficiency caused by too many nodes. This invention effectively reduces the computational cost of optimal trajectory solution while strictly ensuring the motion accuracy of the robotic arm end effector along the specified path, and improves the stability and convergence of the solution process. It is particularly suitable for industrial operations such as laser cutting, spraying, and welding, which have stringent requirements for both path accuracy and motion efficiency. Attached Figure Description
[0036] Figure 1 A flowchart of a step-by-step, multi-stage point-alignment robotic arm trajectory planning method along a specific path provided by the present invention; Figure 2 A schematic flowchart of a step-by-step, multi-stage point-alignment robotic arm trajectory planning method along a specific path, provided for one embodiment; Figure 3 A schematic diagram of discrete arc length nodes and continuous ideal paths provided for a certain embodiment; Figure 4 A block diagram of a step-by-step, multi-stage point-alignment robotic arm trajectory planning system along a specific path, provided by the present invention; Figure 5 A schematic diagram of the hardware structure of a possible electronic device provided by the present invention; Figure 6 This is a schematic diagram of the hardware structure of a possible computer-readable storage medium provided by the present invention. Detailed Implementation
[0037] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.
[0038] Figure 1 A flowchart of a step-by-step, multi-stage point-alignment robotic arm trajectory planning method along a specific path, provided by this invention, is shown below. Figure 1 As shown, the method includes steps S1 to S2: S1, Perform path node planning A geometric path discrete error minimization optimization problem is constructed with the arc length of a specific path at the end of the robotic arm as the independent variable. The problem is solved iteratively by adaptive mesh encryption to obtain key discrete nodes that satisfy a preset discrete error threshold. These key discrete nodes correspond to key positions on the specific path.
[0039] This step constructs a geometric path discretization error minimization optimization problem with a specific path arc length as the independent variable, and employs adaptive mesh refinement for iterative solution. This automatically determines the optimal number and distribution of discrete nodes. This step overcomes the lack of adaptability in traditional manual heuristic setting of the number of discrete stages. It automatically adds nodes in regions with drastic changes in path curvature to ensure fitting accuracy, and automatically reduces nodes in flat regions to avoid redundant computation, thus achieving the preset path error requirement with the minimum number of nodes. This step allows subsequent steps to be performed on a precise and compact discretization basis, avoiding path deviation problems caused by too few nodes, and also avoiding the decision variable inflation and sharp drop in solution efficiency caused by too many nodes.
[0040] S2, Perform optimal trajectory planning Using the key discrete nodes as the stage boundary conditions of the multi-stage optimal control problem, a multi-stage optimal control mathematical model with dynamic constraints is constructed and solved, and the time-optimal or energy-optimal trajectory along the specific path is output.
[0041] Since step S1 has already provided the key discrete nodes that meet the path error requirements, this step does not need to consider the path fitting accuracy issue and can focus on dynamic optimization. This step effectively reduces the difficulty of solving multi-stage optimal control problems, improves the stability and convergence speed of the solution process, and can ultimately efficiently output the time-optimal or energy-optimal trajectory along a specific path. It is particularly suitable for industrial operation scenarios such as laser cutting, spraying, and welding, which have stringent requirements for both path accuracy and motion efficiency.
[0042] Understandably, given the deficiencies in the background technology, this invention proposes a step-by-step, multi-stage point-alignment robotic arm trajectory planning method along a specific path. This method effectively reduces the computational overhead of optimal trajectory solving while strictly ensuring the accuracy of the robotic arm's end effector movement along the specified path, and simultaneously improves the stability and convergence of the solution process. It is particularly suitable for industrial operations such as laser cutting, spraying, and welding, which have stringent requirements for both path accuracy and motion efficiency.
[0043] Based on the above technical solutions, the embodiments of the present invention can be further improved as follows.
[0044] In one possible implementation, step S1 includes sub-steps S101 to S105: S101, taking the arc length of a specific path at the end of the robotic arm as the independent variable, introduces joint space state variables and control variables, sets the performance index as the error integral between the approximate path and the specific path in Cartesian space, and sets differential state constraints, path constraints, boundary conditions and inequality constraints to construct the optimization problem of minimizing the discrete error of the geometric path of the robotic arm; wherein, the specific path is the workpiece contour or predetermined trajectory that the end of the robotic arm needs to follow during laser cutting, spraying, welding and other operations.
[0045] Specifically, this step constructs a geometric path discrete error minimization optimization problem. This problem finds a series of key nodes in the joint space such that the approximate path formed by them is as close as possible to the ideal path in Cartesian space. The specific description of this geometric path discrete error minimization optimization problem is as follows: (1) Minimize the performance metric:
[0046]
[0047] (2) Combined state variables:
[0048] (3) Control variables:
[0049] (4) Differential state constraints:
[0050] (5) Path constraints:
[0051] (6) Boundary conditions:
[0052]
[0053] (7) Inequality constraints:
[0054]
[0055]
[0056] In the formula, Indicates the arc length of a specific path. It is introduced as an additional state variable to connect the mapping relationship between the two spaces, and its differential... It represents linear velocity and is not less than zero. Indicates linear acceleration; Indicates the initial arc length. , and These represent the total arc length, maximum linear velocity, and maximum linear acceleration of the target path, respectively. (Performance metrics here) Let be the integral of the error between the approximate path and the ideal path in Cartesian space, where and These represent approximate and ideal paths in Cartesian space, respectively. In this embodiment, the specific path is considered to be an ideal path.
[0057] Represents the combined state variables. Represents the differential of the combined state variables. Represents the comprehensive differential state function; This represents the forward kinematics function of the robotic arm, used to complete the state transformation from joint space to Cartesian space. This represents the positive dynamics function of the robotic arm. Indicates joint angle, Indicates joint angular velocity, This represents the joint angular acceleration. and These represent the initial and final states of the robotic arm joint angles, respectively. and These represent the initial and final states of the angular velocity of the robotic arm joints, respectively. and These represent the maximum angle and maximum angular velocity that the robotic arm joints can execute, respectively. The torque of the joint motor, This indicates the maximum torque that the joint motor can provide.
[0058] S102, uniformly set the initial set of discrete nodes according to the arc length, and define the joint space function of the specific path.
[0059] In practice, an initial set of discrete nodes is constructed:
[0060] In the formula, the variables include the arc length position of each discrete node. and the corresponding joint space coordinates In the initial set of discrete nodes, the total number of discrete coordinate points is... , This indicates the point number starting from 0.
[0061] Define the joint space function corresponding to the ideal geometric path in Cartesian space:
[0062] In the formula, This represents the inverse kinematics function of the robotic arm.
[0063] Define the approximate path in the joint space obtained by fitting discrete nodes using piecewise interpolation. ,satisfy: .
[0064] S103, using the numerical integration method, the continuous geometric path discrete error minimization optimization problem is discretized into a nonlinear programming problem according to the discrete node set and solved to obtain the approximate path under the current discrete node.
[0065] In practice, the optimization problem in step S101 is discretized into a nonlinear programming problem based on the discrete node set in step S102, as follows: (1) Minimize the performance metric:
[0066] (2) The differential state constraint is:
[0067]
[0068] (3) The path constraints are:
[0069] (4) The boundary conditions are:
[0070]
[0071]
[0072]
[0073]
[0074] (5) The inequality constraints are:
[0075]
[0076]
[0077]
[0078]
[0079]
[0080]
[0081] The total number of discrete collocation points is , This represents the point index starting from 0, with the index being... The collocation of points is represented as , ; This represents constant coefficients in numerical integration methods, which vary depending on the numerical integration method used. Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The joint space coordinates corresponding to each node. Joint space coordinates The Cartesian space approximation path obtained by positive kinematic mapping; Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The arc length of a specific path corresponding to each node. For local intervals The numerical integration method requires further discretization. The th secondary discrete point The specific path corresponding to each node.
[0082] Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The values of the integrated differential state functions corresponding to each node. For local intervals Based on the requirement of discrete generation by numerical integration method The th secondary discrete point The comprehensive state variables corresponding to each node For local intervals Based on the requirement of discrete generation by numerical integration method The th secondary discrete point Control variables corresponding to each node; Indicates matching points The overall state variables at the location, Indicates matching points The overall state variables at the location; , , , , and Indicates matching points The state variables and control variables at the location.
[0083] This step transforms the problem of minimizing discrete errors of continuous geometric paths into a solvable nonlinear programming problem, providing a computable approximate path and error evaluation basis for subsequent adaptive mesh refinement, and is a prerequisite for realizing adaptive adjustment of discrete nodes.
[0084] In this embodiment, the Hermite-Simpson numerical integration method is used to illustrate a nonlinear programming problem. The specific form of the nonlinear programming problem implemented using the Hermite-Simpson numerical integration method is as follows: (1) Minimize the performance metric:
[0085]
[0086]
[0087] (2) Differential state constraints:
[0088]
[0089] in, Indicates the first The arc length (step size) of a segment is numerically equal to the arc length of the endpoint of that segment. Subtract the starting arc length . Indicates the first The Cartesian path error at the segment's starting point, i.e., the actual joint position. with the ideal path point The Euclidean distance between them (expressed using the L2 norm). Indicates the first Section and the Cartesian path error at the midpoint between segments. Indicates the first Cartesian path error at the end of the segment. Indicates the first Joint position variables at discrete nodes Indicates the first The arc length coordinates of a discrete node on the ideal path.
[0090] and They represent the first The system state vectors at the start and end points of the segment (usually containing joint positions and their derivatives). and They represent the first The system state derivative vector (i.e., the rate of change of the state) at the start and end points of the segment. Indicates the first Section and the The system state derivative vector at the midpoint between segments.
[0091] The above problem can be solved using a conventional NLP solver.
[0092] This embodiment uses the Hermite-Simpson numerical integration method to discretize the continuous optimization problem into a nonlinear programming problem. This method can reduce the number of discrete nodes while maintaining high discretization accuracy, thereby obtaining a solution that meets the path error requirements with less computational resources in the subsequent adaptive encryption process.
[0093] S104. Estimate the local discrete path error for each arc length interval. If the local discrete path error is greater than the preset threshold, calculate the number of new discrete nodes to be added based on the error magnitude and insert the new nodes in the arc length interval. After updating the discrete node set, return to step S103 to solve again until the local discrete path error of all arc length intervals is not greater than the preset threshold.
[0094] In practice, the mesh optimization iteration is performed through the following sub-steps S1041~S1044 to update the discrete node set.
[0095] S1041, for each arc length interval Estimate local discrete path error :
[0096] in, Indicates the arc length At this point, the actual joint position vector is obtained based on the current discrete interpolation. Indicates the arc length At this point, the joint position vector is used as a reference on the ideal path.
[0097] Similarly, various numerical integration methods can be used, but here we will summarize them using the mechanical quadrature formula as follows:
[0098] in, Represented in arc length coordinates At that point, the actual joint position vector obtained; Represented in arc length coordinates At this point, the joint position vector on the ideal path serves as a reference; These are constant coefficients in numerical integration methods.
[0099] S1042, the error generated by the numerical integration method satisfies the general formula:
[0100] in, For the numerical method order, It is often referred to as the truncation error coefficient. This represents the step size of the local discrete path, i.e. .
[0101] This step utilizes the local discrete path error from step S1041. Derivation of the truncation error coefficient The approximate value can be obtained, and thus the reasonable value of the local discrete path step size under the ideal local discrete path error requirement can be estimated. The next step is to introduce additional discrete path nodes so that the local discrete path step size meets the requirements.
[0102] S1043, if local error Greater than the preset error threshold If a discrete node is added within this interval, the number of new nodes will be [number missing]. satisfy This means that additional discrete nodes need to be added to the path:
[0103] In the formula, Round up to the nearest integer to update the discrete node set and return to step S102.
[0104] If all local interval errors Then proceed to the next step.
[0105] As one preferred implementation method, firstly, for each arc length interval... The Hermite-Simpson numerical integration method is used to estimate the local discrete path error. :
[0106] in, Indicates the interval of arc length The actual joint position vector at the starting point. Indicates the interval of arc length The actual joint position vector at the endpoint. Indicates the interval of arc length Midpoint The actual joint position vector at the location; Indicates the interval of arc length The starting point processes the joint position vectors along the desired path. Indicates the interval of arc length The endpoint processing involves the joint position vectors along the desired path. Indicates the interval of arc length Midpoint Process the joint position vectors on the desired path.
[0107] Secondly, determine the order of the numerical integration method used, specifically the order of the Hermite-Simpson method. Estimate the truncation error coefficient :
[0108] If the local estimation error is greater than the preset threshold, i.e. In the arc length interval The number of newly added discrete nodes is calculated using the following formula:
[0109] In the formula, Round up.
[0110] The algorithm is in the first an arc length interval Additional introduction The discrete nodes further subdivide the local arc length interval into multiple sub-intervals, thereby achieving local discrete node density and ensuring that the discrete path error of the subdivided sub-arc length intervals meets the requirements.
[0111] If local node densification is performed, all local interval errors Then proceed to step S105.
[0112] S105 outputs a set of key discrete nodes that meet the geometric path discretization error requirements. .
[0113] The solution obtained from one embodiment is as follows: Figure 3 As shown, Figure 3 The solid line shown represents the ideal path, and the black nodes represent the key nodes of the path that meet the requirements of local discrete error. It can be observed that the set of key discrete nodes in the final output is not uniformly distributed, which proves that step S1 uses as few nodes as possible to achieve the control of the target discrete accuracy.
[0114] In one possible implementation, step S2, which involves performing optimal trajectory planning, includes sub-steps S201 to S203.
[0115] S201, using each node in the set of key discrete nodes as the start and end states of each stage, construct boundary conditions and continuity conditions between stages to ensure that the position, velocity and time of adjacent stages are continuous at the connection point.
[0116] In practice, the key discrete node set planned from the path nodes in step S1 is used as the basis for implementation. Constructing inter-stage boundary conditions for a multi-stage optimal control problem: (1) Stage The initial state satisfies: ; (2) Continuity conditions between stages: .
[0117] and These represent the start and end times of the i-th stage, respectively. This represents the target position, derived from the preceding path planning, which serves as the starting state for the i-th stage. This represents the target velocity, derived from the preceding path planning, which serves as the starting state for the i-th stage.
[0118] The above inter-stage boundary conditions indicate the initial position of stage i. and initial velocity It must be equal to the i-th discrete target state derived from the path node planning. and The termination position of stage i and termination speed It must be equal to the first path node derived from the path node planning. Discrete target states and .
[0119] The above continuity condition between stages indicates that the position, velocity, and time at the end of the previous stage (e.g., stage i) must be equal to the position, velocity, and time at the beginning of the next stage (e.g., stage i+1), that is, the position, velocity, and time of adjacent stages at the connection point are continuous, so as to ensure the smooth connection of the entire motion trajectory.
[0120] S202, Based on the aforementioned inter-stage boundary conditions, a complete multi-stage optimal control mathematical model is constructed. This multi-stage optimal control mathematical model includes performance indices, differential constraints, and inequality constraints, wherein: (1) Performance indicators:
[0121] in, For the number of stages, Let i be the boundary index function for stage i. Let i be the process index function for stage i. and These are the start and end times of stage i, respectively. Let i be the state variable of stage i. For stage i, and These are the initial state vector and the final state vector of stage i, respectively. and These are constant weight parameters, and all are non-negative.
[0122] It is worth noting that the boundary index function and process index function It can be customized according to optimization needs.
[0123] Typically, considering time-optimal problems, we can let Considering the energy optimization problem, we can let Based on the boundary conditions, a complete multi-stage optimal control mathematical model can be constructed:
[0124] when Let zero represent the energy optimality problem, when Let zero represent the time-optimal problem, and when both are non-zero, it represents the comprehensive-optimal problem.
[0125] (2) Differential constraints:
[0126] in, , Indicates control variables; (3) Inequality constraints:
[0127] in, This indicates the index of the collocation point for further discrete steps in each stage. This represents the system position variable (e.g., the joint angle of a robot) at the j-th collocation point. This represents the maximum allowed system position at the j-th coordinate point. Let represent the system velocity variable at the j-th collocation point (which is the first derivative of the position). This represents the maximum allowed system velocity at the j-th collocation point. This represents the control variable (e.g., motor torque) at the j-th alignment point. This represents the maximum control input threshold (i.e., the upper limit of each variable) at the j-th coordinate point.
[0128] S203, reference Figure 2 As shown, the optimal control mathematical model is discretized into a nonlinear programming mathematical model using the multi-stage direct collocation method and solved using an NLP solver, outputting the time-optimal or energy-optimal trajectory along the specific path. .
[0129] This embodiment constructs a multi-stage optimization framework with continuity and boundary constraints, decomposing the complex full-process trajectory planning problem into several easily solvable local stages. This significantly reduces the computational complexity of a single optimization while ensuring the smooth and continuous motion trajectory and satisfying physical constraints.
[0130] Figure 4 This invention provides a structural diagram of a step-by-step, multi-stage point-alignment robotic arm trajectory planning system along a specific path, as shown in the embodiment of the invention. Figure 4 As shown, a step-by-step, multi-stage point-alignment robotic arm trajectory planning system along a specific path includes a path node planning module and an optimal trajectory planning module, wherein: The path node planning module is used to construct a geometric path discrete error minimization optimization problem with the arc length of a specific path at the end of the robotic arm as the independent variable. It solves the problem through adaptive mesh densification and iteration to obtain key discrete nodes that meet the preset discrete error threshold. The key discrete nodes correspond to key positions on the specific path. The optimal trajectory planning module is used to construct and solve a multi-stage optimal control mathematical model with dynamic constraints, using the key discrete nodes as stage boundary conditions for the multi-stage optimal control problem, and output the time-optimal or energy-optimal trajectory along the specific path.
[0131] It is understood that the trajectory planning system for a step-by-step multi-stage point-alignment robotic arm along a specific path provided by the present invention corresponds to the trajectory planning method for a step-by-step multi-stage point-alignment robotic arm along a specific path provided in the foregoing embodiments. The relevant technical features of the trajectory planning system for a step-by-step multi-stage point-alignment robotic arm along a specific path can be referred to the relevant technical features of the trajectory planning method for a step-by-step multi-stage point-alignment robotic arm along a specific path, and will not be repeated here.
[0132] Please see Figure 5 , Figure 5 This is a schematic diagram illustrating an embodiment of the electronic device provided in this invention. For example... Figure 5 As shown, this embodiment of the invention provides an electronic device 500, including a memory 510, a processor 520, and a computer program 511 stored in the memory 510 and executable on the processor 520. When the processor 520 executes the computer program 511, it performs the following steps: S1, construct a geometric path discrete error minimization optimization problem with the arc length of a specific path at the end of the robotic arm as the independent variable, and solve it iteratively through adaptive mesh encryption to obtain key discrete nodes that meet the preset discrete error threshold. The key discrete nodes correspond to key positions on the specific path. S2, using the key discrete nodes as the stage boundary conditions of the multi-stage optimal control problem, construct and solve the multi-stage optimal control mathematical model with dynamic constraints, and output the time-optimal or energy-optimal trajectory along the specific path.
[0133] Please see Figure 6 , Figure 6 This is a schematic diagram illustrating an embodiment of a computer-readable storage medium provided by the present invention. (See diagram below.) Figure 6 As shown, this embodiment provides a computer-readable storage medium 600, on which a computer program 511 is stored. When the computer program 511 is executed by a processor, it performs the following steps: S1, construct a geometric path discrete error minimization optimization problem with the arc length of a specific path at the end of the robotic arm as the independent variable, and solve it iteratively through adaptive mesh encryption to obtain key discrete nodes that meet the preset discrete error threshold. The key discrete nodes correspond to key positions on the specific path. S2, using the key discrete nodes as the stage boundary conditions of the multi-stage optimal control problem, construct and solve the multi-stage optimal control mathematical model with dynamic constraints, and output the time-optimal or energy-optimal trajectory along the specific path.
[0134] This invention provides a step-by-step, multi-stage trajectory planning method, system, electronic device, and storage medium for a robotic arm along a specific path. The trajectory planning of the robotic arm's end effector along a specific path is decomposed into two independent and sequentially executed stages. The first stage uses the arc length of the specific path as the independent variable to construct a geometric path discrete error minimization optimization problem. Through adaptive mesh refinement and iterative solving, a set of non-uniformly distributed key discrete nodes is automatically determined. This set of key discrete nodes is densely arranged where the path curvature changes drastically and sparsely distributed in straight sections, thus achieving the preset path error requirement with the fewest nodes. The second stage uses this set of key discrete nodes as the stage boundary conditions for a multi-stage optimal control problem, constructing and solving a multi-stage optimal control mathematical model with dynamic constraints, and outputting the time-optimal or energy-optimal trajectory along the path. This invention completely decouples path geometric accuracy assurance from dynamic optimal solution, eliminating the accuracy loss or computational explosion contradiction caused by improper selection of discrete granularity in traditional methods. The adaptive encryption mechanism automatically matches the number of discrete nodes to the path geometric complexity, effectively reducing the solution scale of subsequent optimal control problems while strictly ensuring end-point tracking accuracy, and improving the stability and convergence speed of the solution process. It is particularly suitable for high-precision industrial scenarios such as laser cutting, spraying, and welding.
[0135] It should be noted that the descriptions of each embodiment in the above embodiments have different focuses. For parts that are not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.
[0136] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.
[0137] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded computer, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.
[0138] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.
[0139] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.
[0140] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.
[0141] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.
Claims
1. A trajectory planning method for a multi-stage point-to-point robot along a specific path, characterized in that, include: S1, construct a geometric path discrete error minimization optimization problem with the arc length of a specific path at the end of the robotic arm as the independent variable, and solve it iteratively through adaptive mesh encryption to obtain key discrete nodes that meet the preset discrete error threshold. The key discrete nodes correspond to key positions on the specific path. S2, using the key discrete nodes as the stage boundary conditions of the multi-stage optimal control problem, construct and solve the multi-stage optimal control mathematical model with dynamic constraints, and output the time-optimal or energy-optimal trajectory along the specific path.
2. The method for trajectory planning along a specific path by a step-by-step, multi-stage point-alignment robotic arm according to claim 1, characterized in that, Step S1 includes: S101, taking the arc length of a specific path at the end of the robotic arm as the independent variable, introduces joint space state variables and control variables, sets the performance index as the error integral between the approximate path and the specific path in Cartesian space, and sets differential state constraints, path constraints, boundary conditions and inequality constraints to construct the optimization problem of minimizing the discrete error of the geometric path of the robotic arm; wherein, the specific path is the workpiece contour or predetermined trajectory that the end of the robotic arm needs to follow during the operation. S102, uniformly set the initial set of discrete nodes according to the arc length, and define the joint space function of the specific path; S103, Using the numerical integration method, the continuous geometric path discrete error minimization optimization problem is discretized into a nonlinear programming problem according to the discrete node set and solved to obtain the approximate path under the current discrete node; S104. Estimate the local discrete path error for each arc length interval. If the local discrete path error is greater than the preset threshold, calculate the number of new discrete nodes to be added based on the error size and insert the new nodes in the arc length interval. After updating the discrete node set, return to step S103 to solve again until the local discrete path error of all arc length intervals is not greater than the preset threshold. S105 outputs the set of key discrete nodes that meet the error requirements.
3. The method for trajectory planning along a specific path by a step-by-step, multi-stage point-alignment robotic arm according to claim 2, characterized in that, In step S101, the minimization performance index of the geometric path discrete error minimization optimization problem is described as follows: Minimize the performance metric, expressed as: The combined state variables are represented as follows: Control variables are represented as: Differential state constraints are expressed as: Path constraints: Boundary conditions: Inequality constraints: in, Joint space coordinates The Cartesian space approximation path obtained through positive kinematic mapping. For a specific path, Let the arc length be , , The initial arc length, The total arc length of the target path; Indicates linear velocity. Indicates the maximum linear velocity. Indicates linear acceleration. Indicates the maximum linear acceleration; Represents the combined state variables. Represents the differential of the combined state variables. Represents the comprehensive differential state function. Represents the forward kinematics function of the robotic arm. Represents the positive dynamics function of the robotic arm; Indicates joint angle, Indicates joint angular velocity, Represents joint angular acceleration. and These represent the initial and final states of the robotic arm joint angles, respectively. and These represent the initial and final states of the angular velocity of the robotic arm joints, respectively. and These represent the maximum angle and maximum angular velocity that the robotic arm joints can execute, respectively. The torque of the joint motor, This indicates the maximum torque that the joint motor can provide.
4. The method for trajectory planning along a specific path by a step-by-step, multi-stage point-alignment robotic arm according to claim 3, characterized in that, In step S103, the performance index of the discretized nonlinear programming problem is expressed as: The differential state constraint is: The total number of discrete collocation points is , This indicates the point number starting from 0. ; Represents constant coefficients in numerical integration methods; Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The joint space coordinates corresponding to each node. Joint space coordinates The Cartesian space approximation path obtained by positive kinematic mapping; Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The arc length of a specific path corresponding to each node. For local intervals The numerical integration method requires further discretization. The th secondary discrete point The specific path corresponding to each node; Indicates local interval The numerical integration method requires further discretization. The th secondary discrete point The values of the integrated differential state functions corresponding to each node. For local intervals Based on the requirement of discrete generation by numerical integration method The th secondary discrete point The comprehensive state variables corresponding to each node For local intervals Based on the requirement of discrete generation by numerical integration method The th secondary discrete point Control variables corresponding to each node; Indicates matching points The overall state variables at the location, Indicates matching points The overall state variables at the location.
5. The method for trajectory planning along a specific path by a step-by-step, multi-stage point-alignment robotic arm according to claim 2, characterized in that, In step S104, the method for estimating the local discrete path error is as follows: Using the same numerical integration method as in step S103, calculate the integral of the deviation between the approximate path and the specific path in joint space over each arc length interval.
6. The method for trajectory planning along a specific path by a step-by-step, multi-stage point-alignment robotic arm according to claim 2, characterized in that, In step S104, the formula for calculating the number of newly added discrete nodes is: in, This represents the local error within the current interval. To preset the error threshold, Let be the order of the numerical integration method used. Rounded up, the result is the number of new nodes.
7. A method for trajectory planning along a specific path for a step-by-step, multi-stage point-alignment robotic arm according to any one of claims 1 to 6, characterized in that, Step S2 includes: S201, using each node in the set of key discrete nodes as the start and end states of each stage, construct the boundary conditions and continuity conditions between stages to ensure that the position, velocity and time of adjacent stages are continuous at the connection point. S202, Based on the inter-stage boundary conditions, construct a complete multi-stage optimal control mathematical model, which includes performance indicators, differential constraints, and inequality constraints; S203, the optimal control mathematical model is discretized into a nonlinear programming mathematical model using the multi-stage direct collocation method and solved to output the time-optimal or energy-optimal trajectory along the specific path.
8. The method for trajectory planning along a specific path by a step-by-step, multi-stage point-alignment robotic arm according to claim 7, characterized in that, In step S201, the inter-stage continuity conditions include: The position, velocity, and time are continuous at the connection point between adjacent stages.
9. The method for trajectory planning along a specific path by a step-by-step, multi-stage point-alignment robotic arm according to claim 7, characterized in that, In step S202, the performance indicators of the multi-stage optimal control mathematical model include: in, For the number of stages, Let i be the boundary index function for stage i. Let i be the process index function for stage i. and These are the start and end times of stage i, respectively. Let i be the state variable of stage i. For stage i, and These are the initial state vector and the final state vector of stage i, respectively. and For weights.
10. A trajectory planning system for a step-by-step, multi-stage point-alignment robotic arm along a specific path, characterized in that, include: The path node planning module is used to construct a geometric path discrete error minimization optimization problem with the arc length of a specific path at the end of the robotic arm as the independent variable. It solves the problem through adaptive mesh densification and iteration to obtain key discrete nodes that meet the preset discrete error threshold. The key discrete nodes correspond to key positions on the specific path. The optimal trajectory planning module is used to construct and solve a multi-stage optimal control mathematical model with dynamic constraints, using the key discrete nodes as stage boundary conditions for the multi-stage optimal control problem, and output the time-optimal or energy-optimal trajectory along the specific path.
