A six-axis robot trajectory optimization method fusing kinematic constraints and parameterized simulation verification

Abstract

The application provides a six-axis robot trajectory optimization method combining kinematics constraints and parameterized simulation verification, and relates to the technical field of robot trajectory planning, and comprises the following steps: determining a path segment to be optimized of a six-axis robot based on entity hardware composition of a workstation, signal control logic and process beat requirements; constructing a six-axis robot trajectory planning model; generating an optimized trajectory of an end effector of the six-axis robot by adopting a quintic polynomial interpolation algorithm; performing process point selection on the optimized trajectory of the end effector of the six-axis robot, and performing risk simulation verification on the selected process points by utilizing a mixed collision early warning mechanism of the separation axis theorem and a neural symbolic distance field, until the optimized trajectory of the end effector of the six-axis robot does not have an interference risk, and finally obtaining the end effector trajectory of the six-axis robot. The application can avoid hardware loss caused by equipment collision and raw material waste in the debugging process.

Description

Technical Field

[0001] This invention relates to the field of robot trajectory planning technology, and more specifically, to a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification. Background Technology

[0002] Optimizing the path of a six-axis robot not only directly improves the efficiency, quality, and safety of the production line, but also extends equipment life, reduces operating costs, and supports flexible production. It is a key link for six-axis robots to fully adapt to the needs of intelligent manufacturing. In the field of robot trajectory optimization, existing technologies mainly use a hybrid method of RRT algorithm combined with six-axis B-spline curve optimization for path planning. The working principle is as follows: 1) Improved RRT algorithm path planning: First, initialize a random tree containing the starting point. Generate trial points through random sampling with target bias. Expand new nodes from the nearest node in the tree with an adaptive step size. After collision detection, add them to the tree. If the new node reaches the target area, backtrack to obtain the initial path; otherwise, repeat sampling and expansion. 2) Six-axis B-spline curve optimization: First, select control points from the initial path. Combine curvature and obstacle secondary optimization to reduce the number and improve smoothness and adaptability. Then, use these control points to fit the curve, adjust the position to meet the robot's kinematic and dynamic constraints, and fit the initial path.

[0003] However, this method still faces significant bottlenecks in practical applications. First, poor initial path quality limits its optimization potential. Improved RRT is prone to generating initial paths with many turns and high redundancy in narrow passages or obstacle-dense scenarios. Even with B-spline smoothing, it is still difficult to completely avoid the risk of implicit collisions between robot joints and obstacles. In addition, this method lacks a verification mechanism that is deeply integrated with simulation platforms. Traditional methods often perform path planning and optimization outside of high-fidelity simulation environments, making it impossible to verify the effectiveness of the trajectory throughout the entire process and across multiple indicators before implementation. This results in high risks for engineering implementation and long debugging cycles.

[0004] No effective solutions have yet been proposed to address the problems in the relevant technologies. Summary of the Invention

[0005] In view of this, the present invention provides a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification to solve the aforementioned problems.

[0006] To solve the above problems, the specific technical solution adopted by the present invention is as follows:

[0007] A trajectory optimization method for a six-axis robot that integrates kinematic constraints and parametric simulation verification includes:

[0008] S1. Based on the physical hardware composition, signal control logic, and process cycle requirements of the workstation, and combined with the preset mapping requirements, construct a virtual simulation model consistent with the physical scene through parametric modeling; use the virtual simulation model to determine the path segment to be optimized for the six-axis robot.

[0009] S2. Based on the geometric structure and kinematic constraints of the six-axis robot, and through an integrated trajectory planning algorithm that incorporates kinematic constraints, a trajectory planning model for the six-axis robot is constructed.

[0010] S3. Based on the path segment to be optimized for the six-axis robot, the virtual simulation model, and the trajectory planning model of the six-axis robot, and combined with the standardized coordinate transformation mechanism, the optimized trajectory of the six-axis robot end effector is generated using a fifth-order polynomial interpolation algorithm.

[0011] S4. Select process points for the optimized trajectory of the six-axis robot end effector, and use the hybrid collision warning mechanism of the separation axis theorem and the neural symbol distance field to perform risk simulation verification on the selected process points until there is no interference risk in the optimized trajectory of the six-axis robot end effector, and obtain the final trajectory of the six-axis robot end effector.

[0012] Preferably, the virtual simulation model consistent with the physical scene is constructed through parametric modeling based on the physical hardware composition, signal control logic, and process cycle requirements of the workstation, combined with preset mapping requirements; the determination of the path segment to be optimized for the six-axis robot using the virtual simulation model includes the following steps:

[0013] S11. Collect the geometric parameters of each device in the workstation and the motion parameters of the six-axis robot, and construct a virtual simulation model based on the mapping requirements of geometric parameters and virtual model.

[0014] S12. Assign functional attributes to each device in the virtual simulation model that are consistent with the physical entity, and based on the process cycle of the physical production line, map the sequence of device actions and interaction logic through process replication and write control instructions to achieve a complete match between the virtual process and the actual production process, so as to obtain a virtual simulation model that is consistent with the physical scene.

[0015] S13. Based on the operational requirements of the six-axis robot in the workstation, determine the initial motion path of the six-axis robot in the virtual simulation model;

[0016] S14. Based on the virtual simulation model, the actual production process of the six-axis robot is simulated by running a virtual process, and the path segments with equipment interference and cycle mismatch problems are obtained and marked by detecting them.

[0017] Preferably, the process of constructing a trajectory planning model for a six-axis robot based on its geometric structure and kinematic constraints, and through an integrated trajectory planning algorithm that incorporates kinematic constraints, includes the following steps:

[0018] S21. Obtain the physical link structure of the six-axis robot, establish a local coordinate system for each link and extract the standard DH parameters;

[0019] S22. Based on standard DH parameters and combined with preset virtual six-degree-of-freedom micro-motion joints, construct an extended parametric kinematic mathematical model containing compensable parameter vectors.

[0020] S23. Perform global sampling in the workspace of the six-axis robot and stack the Jacobian matrix based on the sampling results. By performing spectral analysis and ill-conditioning on the Jacobian matrix, identify the key parameter combinations that cause instability in the solution of the end pose error of the six-axis robot.

[0021] S24. Measure the actual end pose of multiple calibration points in the workspace of the six-axis robot, and construct a nonlinear least squares optimization model for key parameter combinations with the goal of minimizing the sum of squared residuals between the actual end pose and the theoretical pose calculated in advance by the extended parameterized kinematic mathematical model.

[0022] S25. The Levenberg-Marquardt algorithm is used to iteratively solve the nonlinear least squares optimization model to obtain the enhanced DH parameter set after error compensation.

[0023] S26. The enhanced DH parameter set after error compensation is used as the model parameters of the extended parametric kinematic mathematical model, and the extended parametric kinematic mathematical model is imported into the simulation software to instantiate and generate the corresponding six-axis robot trajectory planning model.

[0024] Preferably, the step of generating the optimized trajectory of the six-axis robot end effector using a fifth-order polynomial interpolation algorithm based on the path segment to be optimized for the six-axis robot, the virtual simulation model, and the trajectory planning model of the six-axis robot, and in conjunction with a standardized coordinate transformation mechanism, includes the following steps:

[0025] S31. Perform initial attitude alignment between the virtual simulation model and the six-axis robot trajectory planning model so that the coordinate system of the six-axis robot end effector corresponds in the three-axis direction in the virtual simulation model and the six-axis robot trajectory planning model;

[0026] S32. After initial posture alignment, calculate the coordinate difference between the six-axis robot end effector in the initial posture in the virtual simulation model and the six-axis robot trajectory planning model.

[0027] S33. Based on coordinate difference, construct a standardized coordinate transformation formula for converting coordinates in a six-axis robot trajectory planning model into coordinates in a virtual simulation model;

[0028] S34. Determine the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for the six-axis robot, and based on the boundary conditions and total running time of the six-axis robot, use a fifth-order polynomial interpolation algorithm to determine the position of each trajectory point of the end effector of the six-axis robot, thus obtaining the optimized trajectory of the end effector of the six-axis robot.

[0029] Preferably, the process of determining the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for the six-axis robot, and determining the position of each trajectory point of the six-axis robot end effector using a fifth-order polynomial interpolation algorithm based on the boundary conditions and total running time of the six-axis robot, to obtain the optimized trajectory of the six-axis robot end effector, includes the following steps:

[0030] S341. Based on the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for a six-axis robot, construct a position function that includes the changes in position, velocity, and acceleration over time.

[0031] S342. Based on the boundary conditions of the six-axis robot end effector and the total running time, construct a system of simultaneous boundary equations;

[0032] S343. Solve the simultaneous boundary equations, substitute the solution back into the position function, and use the fifth-order polynomial interpolation algorithm to solve for the pose information of the six-axis robot end effector.

[0033] S344. Connect the pose information of the six-axis robot end effector in chronological order to form the motion trajectory of the six-axis robot end effector.

[0034] Preferably, the steps of solving the simultaneous boundary equations, substituting the solution back into the position function, and using a fifth-order polynomial interpolation algorithm to solve for the pose information of the six-axis robot end effector include the following steps:

[0035] S3431. Substitute the solution of the simultaneous boundary equations back into the position function, and at any time within the preset time range, use the fifth-order polynomial interpolation algorithm to perform interpolation processing to obtain the discrete pose points of the six-axis robot end effector that vary along the time.

[0036] S3432. For the discrete pose points of the end effector of the six-axis robot, and combined with the start and end points of the path segment to be optimized of the six-axis robot, a cubic Bézier curve is used to fit the path to generate an optimized trajectory that satisfies the minimum curvature or the shortest path.

[0037] S3433. Based on the optimized trajectory that satisfies the minimum curvature or shortest path, the position and attitude information of the end effector of the six-axis robot in the task space are calculated in real time using the forward kinematics algorithm.

[0038] Preferably, the process of selecting process points for the optimized trajectory of the six-axis robot end effector and using a hybrid collision warning mechanism based on the separation axis theorem and neural symbol distance field to perform risk simulation verification on the selected process points until the optimized trajectory of the six-axis robot end effector has no interference risk, and obtaining the final trajectory of the six-axis robot end effector includes the following steps:

[0039] S41. Based on the selection method of multimodal feature point extraction and adaptive cubic spline interpolation, select several process points from the optimized trajectory of the end effector of a six-axis robot, and record the coordinates of each process point and the corresponding joint angle.

[0040] S42. Using the coordinate transformation mechanism, convert the selected process points into the coordinate system of the virtual simulation model, replace the process points of the initial motion path selected in the virtual simulation model, and rerun the virtual process.

[0041] S43. In the rerun of the virtual process, the separation axis theorem is used to perform preliminary interference detection on the six-axis robot to obtain interference detection results, which include the presence of interference risk and the absence of interference risk.

[0042] S44. When the interference detection result indicates that there is an interference risk, the implicit collision detection method based on the neural symbol distance field is used to perform collision detection and optimize the trajectory adjustment to obtain the final trajectory of the six-axis robot end effector.

[0043] S45. When the interference detection result indicates that there is no risk of interference, the optimized trajectory of the six-axis robot end effector shall be taken as the final trajectory of the six-axis robot end effector.

[0044] Preferably, the step of performing preliminary interference detection on the six-axis robot using the separation axis theorem during the rerun of the virtual process to obtain the interference detection results includes the following steps:

[0045] S431. In the rerun of the virtual process, for each discrete pose in the optimized trajectory of the six-axis robot end effector, calculate the minimum bounding box of each link.

[0046] S432. Using the separation axis theorem, we can detect the intersection of the minimum bounding box of each link with the minimum bounding box of all obstacles in the environment, and obtain the detection results.

[0047] S433. Determine whether the detection result meets the preset interference conditions. If it does, determine that there is interference, identify the path segment where the interference pose is located, and merge and generate a subset of connected paths to be finely calculated. If it does not meet the conditions, determine that there is no interference.

[0048] Preferably, when the interference detection result indicates an interference risk, the implicit collision detection method based on the neural symbol distance field is used for collision detection and trajectory optimization to obtain the final six-axis robot end effector trajectory, which includes the following steps:

[0049] S441. When the interference detection result indicates that there is an interference risk, a set of sampling points is generated on the surface of each link of the six-axis robot according to a preset density based on the subset of paths to be precisely calculated.

[0050] S442. Using the principle of forward kinematics, transform the sampling point set to the global coordinate system;

[0051] S443. Construct and train the neural symbol distance field model, and input the sample point set transformed to the global coordinate system into the neural symbol distance field model to obtain the symbol distance value;

[0052] S444. Based on the symbol distance value, the minimum distance of all sampling points in the sampling point set is statistically analyzed. If the minimum distance of the sampling points is less than the preset safety threshold, it is determined that there is a collision risk. The path control points in the automatically adjusted and optimized trajectory are adjusted locally until the preset safety threshold is met. Otherwise, it means that there is no collision risk.

[0053] Preferably, the construction and training of the neural symbol distance field model includes the following steps:

[0054] The point cloud data of the workstation environment is collected using a 3D scanning device, and the symbolic distance to the nearest obstacle surface point is calculated for each sampling point in the point cloud data.

[0055] The point cloud coordinates are normalized, and data augmentation is performed using random rotation, translation, and scaling to obtain augmented point cloud data.

[0056] A multilayer perceptron with position coding is used as an approximator for the symbolic distance function;

[0057] Using the enhanced point cloud data and a pre-defined composite loss function, the multilayer perceptron is trained to obtain a neural symbolic distance field model.

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

[0059] 1. Cost and risk control: The entire process of path optimization and verification is based on a virtual environment. The real scene is reproduced through parametric simulation modeling algorithm. There is no need for trial production of physical equipment, which can avoid hardware damage caused by equipment collision and waste of raw materials during the debugging process.

[0060] 2. Significantly Improved Accuracy and Reliability: Through virtual joints and error compensation mechanisms, this invention not only optimizes the trajectory for "smoothing" and "collision-free" performance, but also achieves "accuracy pre-compensation" optimization based on the actual geometric accuracy of the individual robot. The generated trajectory can directly adapt to the real geometric characteristics of a specific robot, reducing trajectory execution deviations and potential risks caused by inaccurate models.

[0061] 3. Flexibility and adaptability: This invention can quickly adapt to the optimization needs of different models of six-axis robots and the planning needs of different scenarios by updating the hardware parameters and process logic of the virtual model and adjusting the start / end points of trajectory planning, thus meeting the production changeover needs of flexible production.

[0062] 4. Quantifiable and Visualizable Effects: This invention can directly record quantitative indicators such as running time and joint parameters before and after optimization through virtual simulation. At the same time, it presents path trajectory and three-axis trajectory graph through trajectory visualization algorithm, which facilitates evaluation of optimization effect and provides data support for subsequent iterative optimization. Attached Figure Description

[0063] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. In the drawings:

[0064] Figure 1 This is a flowchart of a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification according to an embodiment of the present invention;

[0065] Figure 2 This is a flowchart of an integrated trajectory planning algorithm in a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification according to an embodiment of the present invention.

[0066] Figure 3 This is a virtual simulation workstation diagram in a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification according to an embodiment of the present invention;

[0067] Figure 4 This is a simulation model diagram of a six-axis robot in a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification according to an embodiment of the present invention;

[0068] Figure 5 This is a motion trajectory diagram of the end effector of a six-axis robot in a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification according to an embodiment of the present invention;

[0069] Figure 6 This is a two-dimensional time-domain trajectory diagram in a six-axis robot trajectory optimization method that integrates kinematic constraints and parametric simulation verification according to an embodiment of the present invention. Detailed Implementation

[0070] To enable those skilled in the art to better understand the technical solutions in this application, the technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this application.

[0071] According to an embodiment of the present invention, a method for optimizing the trajectory of a six-axis robot that integrates kinematic constraints and parametric simulation verification is provided.

[0072] Specifically, this invention aims to overcome the technical bottlenecks in six-axis robot path optimization schemes, namely, "limited initial path quality and inability to eliminate hidden collision risks" and "lack of a high-fidelity virtual verification mechanism for optimization results." It proposes a path simulation optimization method for six-axis robots, achieving the following objectives by constructing a complete technical system encompassing parametric simulation modeling, kinematic constraint trajectory planning, and optimization effect verification:

[0073] 1) Establish a virtual simulation workstation to fundamentally improve the environmental realism and verification reliability of path planning. A simulation model of the physical workstation is generated based on the actual workstation's process flow and dimensional parameters, thereby replicating the geometric characteristics and process logic of the workstation equipment and providing an accurate simulation platform for subsequent verification of optimization effects.

[0074] 2) Design an integrated trajectory planning algorithm that incorporates kinematic constraints to overcome the bottleneck of initial path quality and generate scientifically controllable optimized paths. This involves organically combining robot modeling, pose smoothing interpolation, and inverse kinematics solving to generate optimized paths that are smooth in joint space and controllable in task space. This overcomes the shortcomings of traditional methods, such as low optimization upper limits and insufficient safety due to initial path redundancy.

[0075] Based on a standardized coordinate transformation mechanism and a process point selection method that combines multimodal feature point extraction with adaptive cubic spline interpolation algorithm, the optimized path is accurately mapped to the simulation environment. Through quantitative indicators and visual comparison, the improvement effect of the trajectory in terms of smoothness and efficiency is comprehensively evaluated, ensuring that the optimization results have reproducible and verifiable engineering effects.

[0076] The present invention will now be further described in conjunction with the accompanying drawings and specific embodiments, such as... Figures 1-3 As shown, according to an embodiment of the present invention, a six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification is provided, comprising:

[0077] S1. Based on the physical hardware composition, signal control logic, and process cycle requirements of the workstation, and combined with the preset mapping requirements, construct a virtual simulation model consistent with the physical scene through parametric modeling; use the virtual simulation model to determine the path segment to be optimized for the six-axis robot.

[0078] The principle of parametric simulation restoration is to collect the hardware parameters and process logic of the physical workstation, and use a three-layer mapping of "parameter-model-process" to construct a three-in-one virtual model of "geometry-motion-logic" to realize the full process replication of the physical production line. Its core is to ensure the consistency between the virtual environment and the physical environment through parametric modeling and process replication, and to provide a real "test scenario" for path optimization.

[0079] In a preferred embodiment, the virtual simulation model consistent with the physical scene is constructed through parametric modeling based on the physical hardware composition, signal control logic, and process cycle requirements of the workstation, combined with preset mapping requirements; the determination of the path segment to be optimized for the six-axis robot using the virtual simulation model includes the following steps:

[0080] S11. Collect the geometric parameters of each device in the workstation and the motion parameters of the six-axis robot, and construct a virtual simulation model based on the mapping requirements of the geometric parameters and the virtual model; such as Figure 4 As shown;

[0081] S12. Assign functional attributes to each device in the virtual simulation model that are consistent with the physical entity, and based on the process cycle of the physical production line, map the sequence of device actions and interaction logic through process replication and write control instructions to achieve a complete match between the virtual process and the actual production process, so as to obtain a virtual simulation model that is consistent with the physical scene.

[0082] S13. Based on the operational requirements of the six-axis robot in the workstation, determine the initial motion path of the six-axis robot in the virtual simulation model;

[0083] S14. Based on the virtual simulation model, the actual production process of the six-axis robot is simulated by running a virtual process, and the path segments with equipment interference and cycle mismatch problems are obtained and marked by detecting them.

[0084] Specifically, when constructing a virtual simulation model, based on the physical hardware components of the workstation, including robots, processing equipment, and conveying devices, as well as signal control logic and process cycle requirements, a virtual simulation model consistent with the physical scene is constructed through parametric modeling. This includes:

[0085] 1) Model parameter acquisition and 3D reconstruction: Collect the geometric parameters (dimensions, assembly relationships) of each device in the workstation and the motion parameters (joint motion range, speed limits) of the six-axis robot, and construct a high-precision virtual model based on the 1:1 mapping of "geometric parameters - virtual model";

[0086] 2) Equipment attribute definition and process mapping: Assign functional attributes to each device in the virtual model that are consistent with the physical entity, such as the joint motion characteristics of the robot and the opening and closing logic of the gripper. Based on the process cycle of the physical production line, the sequence of equipment actions and interaction logic are mapped through process replication, and control instructions are written to ensure that the virtual process is completely matched with the actual production process.

[0087] 3) Initial path design and conflict pre-detection: Based on the operational requirements of the six-axis robot, such as gripping, handling, and assembly, an initial motion path is designed. The actual production process is simulated through virtual process operation, and equipment interference and cycle mismatch issues in the path are detected. Path segments to be optimized are marked.

[0088] S2. Based on the geometric structure and kinematic constraints of the six-axis robot, and through an integrated trajectory planning algorithm that incorporates kinematic constraints, a trajectory planning model for the six-axis robot is constructed.

[0089] The kinematic trajectory planning principle is based on establishing the mathematical expression of the robot using the DH parameter model. It achieves a bidirectional mapping between "end-effector pose and joint angles" through inverse kinematics algorithms, and leverages the smoothness advantage of fifth-order polynomial interpolation to generate trajectories that satisfy kinematic constraints, such as joint angle, velocity, and acceleration limits. Its core is to achieve a smooth, controllable optimized path in joint space and task space through an integrated trajectory planning algorithm that incorporates kinematic constraints.

[0090] As a preferred embodiment, the process of constructing a trajectory planning model for a six-axis robot based on its geometric structure and kinematic constraints, and through an integrated trajectory planning algorithm that incorporates kinematic constraints, includes the following steps:

[0091] S21. Obtain the physical link structure of the six-axis robot, establish a local coordinate system for each link and extract the standard DH parameters;

[0092] S22. Based on standard DH parameters and combined with preset virtual six-degree-of-freedom micro-motion joints, construct an extended parametric kinematic mathematical model containing compensable parameter vectors.

[0093] S23. Perform global sampling in the workspace of the six-axis robot and stack the Jacobian matrix based on the sampling results. By performing spectral analysis and ill-conditioning on the Jacobian matrix, identify the key parameter combinations that cause instability in the solution of the end pose error of the six-axis robot.

[0094] S24. Measure the actual end pose of multiple calibration points in the workspace of the six-axis robot, and construct a nonlinear least squares optimization model for key parameter combinations with the goal of minimizing the sum of squared residuals between the actual end pose and the theoretical pose calculated in advance by the extended parameterized kinematic mathematical model.

[0095] S25. The Levenberg-Marquardt algorithm is used to iteratively solve the nonlinear least squares optimization model to obtain the enhanced DH parameter set after error compensation.

[0096] S26. The enhanced DH parameter set after error compensation is used as the model parameters of the extended parametric kinematic mathematical model, and the extended parametric kinematic mathematical model is imported into the simulation software to instantiate and generate the corresponding six-axis robot trajectory planning model.

[0097] It should be noted that, based on the selected path to be optimized and the robot's geometry and kinematic constraints, a virtual simulation model of the six-axis robot is constructed and an optimized trajectory is generated through an integrated trajectory planning algorithm that incorporates kinematic constraints. Specifically, this includes:

[0098] DH parameter modeling and error compensation: To improve the adaptability of virtual simulation models to geometric errors of actual robots, such as link deflection, joint clearance, and assembly tolerances, this invention introduces an improved virtual joint modeling and parameter sensitivity analysis compensation mechanism before standard DH modeling. Specifically, this includes:

[0099] (1) Basic Kinematic Framework and Virtual Joint Extended Modeling: A six-axis robot model was selected, its physical structure was determined, a local coordinate system was established for each link, and standard DH parameters were extracted: link length a, link torsion angle α, link offset d, and joint angle θ. Based on this, a virtual six-degree-of-freedom micro-joint was introduced between the end effector flange and the tool (TCP) of the six-axis robot, and between the base and the first joint. The initial DH parameters of these joints were all set to 0, thereby constructing an extended parametric kinematic mathematical model F(p,q) containing a compensable parameter vector p, where q is the joint angle vector. This provides a mathematical basis for subsequent analysis and optimization.

[0100] (2) To efficiently focus on the compensation target, a joint analysis method based on Jacobian matrix singular value decomposition and condition number evaluation is adopted. By sampling the entire workspace and stacking the Jacobian matrix, spectral analysis and ill-condition diagnosis are performed to identify the key parameter combination that is most sensitive to end-effector pose error and is prone to causing solution instability, namely the high-sensitivity parameter subset, which usually includes the offset d of the virtual joint at the end, the torsion angle α, and the actual joint link length a near the end.

[0101] (3) Using a high-precision laser tracker, within the robot's workspace, preferably in areas sensitive to changes in high-sensitivity parameters, the actual end-effector poses at multiple calibration points are measured. A nonlinear least-squares optimization model is established for the subset of high-sensitivity parameters, aiming to minimize the sum of squared residuals between the measured pose and the model-predicted pose. The Levenberg-Marquardt algorithm is used for iterative solution, by constructing and solving the following equations:

[0102] ;

[0103] By adaptively adjusting the damping factor λ, the optimized parameter values ​​are finally output, achieving accurate compensation for the kinematic model. This yields an enhanced DH parameter set after error compensation.

[0104] In the formula, Let be the Jacobian matrix of the residuals with respect to the parameters; This is the residual vector under the current parameters; Let be the damping factor for the k-th iteration; It is an identity matrix with the same dimensions as the parameter vector p; For the parameter increment to be determined;

[0105] (4) The compensated enhanced DH parameter set obtained in the above steps is used as the final kinematic model parameters and imported into the simulation software, such as MATLAB Robotics Toolbox, to instantiate the corresponding robot simulation model object.

[0106] S3. Based on the path segment to be optimized for the six-axis robot, the virtual simulation model, and the trajectory planning model of the six-axis robot, and combined with the standardized coordinate transformation mechanism, the optimized trajectory of the six-axis robot end effector is generated using a fifth-order polynomial interpolation algorithm.

[0107] The coordinate collaboration principle establishes a coordinate transformation relationship between two models through an initial attitude alignment algorithm and a translation compensation method, thereby achieving accurate data transfer. Its core is to eliminate coordinate deviations between different models, ensuring that the optimized trajectory can be directly applied in virtual simulation and avoiding optimization failure caused by data gaps.

[0108] As a preferred embodiment, the step of generating the optimized trajectory of the six-axis robot end effector using a fifth-order polynomial interpolation algorithm based on the path segment to be optimized for the six-axis robot, the virtual simulation model, and the trajectory planning model of the six-axis robot, and in conjunction with a standardized coordinate transformation mechanism, includes the following steps:

[0109] S31. Perform initial attitude alignment between the virtual simulation model and the six-axis robot trajectory planning model so that the coordinate system of the six-axis robot end effector corresponds in the three-axis direction in the virtual simulation model and the six-axis robot trajectory planning model;

[0110] S32. After initial posture alignment, calculate the coordinate difference between the six-axis robot end effector in the initial posture in the virtual simulation model and the six-axis robot trajectory planning model.

[0111] S33. Based on coordinate difference, construct a standardized coordinate transformation formula for converting coordinates in a six-axis robot trajectory planning model into coordinates in a virtual simulation model;

[0112] The high-sensitivity parameter error compensation principle involves introducing parameterized virtual joints to construct an extended kinematic model. Combined with high-precision measurement data, parameter sensitivity analysis is used to identify key factors affecting end-effector accuracy, and then targeted compensation is applied. Its core is to utilize a data-driven approach to incorporate geometric errors that are difficult to model precisely into the corrected model parameters, allowing the theoretical model to approximate the true kinematic characteristics of the physical entity.

[0113] It should be noted that, to ensure accurate and lossless transfer of path point information between the virtual simulation model and the six-axis robot trajectory planning model, this invention establishes a standardized end effector coordinate transformation mechanism. Specifically, this includes:

[0114] 1. Initial pose alignment: First, set the six-axis robot model in the virtual simulation model and the trajectory planning model to have the same initial pose.

[0115] 2. Axial correspondence: Ensure that the robot end effector is strictly corresponding in the three axes of the coordinate system in the two models.

[0116] 3. Difference Calculation: Calculate the coordinate difference between the robot end effector in the initial posture in the two models. When the robot itself does not move in space, i.e., without external translation devices such as slide rails, the coordinate difference between any identical points on the two models remains constant. .

[0117] 4. Dynamic Compensation and Adjustment Mechanism: Since the established robot kinematic model is usually in a fixed position, when the target six-axis robot model to be optimized changes, or the robot is moved as a whole, causing its initial position relative to the global coordinate system to change, the virtual simulation robot must be readjusted to its initial posture and the coordinate difference must be recalculated. If there is equipment translation, translation compensation is needed to compensate for the coordinates in the translation direction to ensure accurate mapping of coordinates between the two models.

[0118] The construction of the coordinate transformation calculation formula includes:

[0119] Assumption: Coordinates of points in the trajectory planning model ( ). Coordinates of points in the virtual simulation model ( ). . If there is no translation, then X=0 Y=0 Z=0.

[0120] 1. Definition of initial attitude difference ( );

[0121] ;

[0122] ;

[0123] ;

[0124] in, These represent the coordinates of the robot when it is in the same initial posture.

[0125] 2. Standardized coordinate transformation formulas can convert trajectory planning coordinates into virtual simulation coordinates.

[0126] The standardized coordinate transformation formula is:

[0127] ;

[0128] After unfolding:

[0129] ;

[0130] ;

[0131] ;

[0132] S34. Determine the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for the six-axis robot, and based on the boundary conditions and total running time of the six-axis robot, use a fifth-order polynomial interpolation algorithm to determine the position of each trajectory point of the end effector of the six-axis robot, thus obtaining the optimized trajectory of the end effector of the six-axis robot.

[0133] In a preferred embodiment, the steps of determining the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for the six-axis robot, and determining the position of each trajectory point of the six-axis robot end effector using a fifth-order polynomial interpolation algorithm based on the boundary conditions and total running time of the six-axis robot to obtain the optimized trajectory of the six-axis robot end effector include the following steps:

[0134] S341. Based on the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for a six-axis robot, construct a position function that includes the changes in position, velocity, and acceleration over time.

[0135] S342. Based on the boundary conditions of the six-axis robot end effector and the total running time, construct a system of simultaneous boundary equations;

[0136] S343. Solve the simultaneous boundary equations, substitute the solution back into the position function, and use the fifth-order polynomial interpolation algorithm to solve for the pose information of the six-axis robot end effector.

[0137] In a preferred embodiment, the steps of solving the simultaneous boundary equations, substituting the solution back into the position function, and using a fifth-order polynomial interpolation algorithm to solve for the pose information of the six-axis robot end effector include the following steps:

[0138] S3431. Substitute the solution of the simultaneous boundary equations back into the position function, and at any time within the preset time range, use the fifth-order polynomial interpolation algorithm to perform interpolation processing to obtain the discrete pose points of the six-axis robot end effector that vary along the time.

[0139] S3432. For the discrete pose points of the end effector of the six-axis robot, and combined with the start and end points of the path segment to be optimized of the six-axis robot, a cubic Bézier curve is used to fit the path to generate an optimized trajectory that satisfies the minimum curvature or the shortest path.

[0140] S3433. Based on the optimized trajectory that satisfies the minimum curvature or shortest path, the position and attitude information of the end effector of the six-axis robot in the task space are calculated in real time using the forward kinematics algorithm.

[0141] S344. Connect the pose information of the six-axis robot end effector in chronological order to form the motion trajectory of the six-axis robot end effector.

[0142] It should be noted that when generating the optimized trajectory and trajectory image, an enhanced DH parameter model with error compensation can be used, based on the three-dimensional spatial coordinates of the starting and ending points of the path to be optimized, boundary conditions, and total running time. The position of each trajectory point is determined using a fifth-order polynomial interpolation algorithm. The specific implementation process is as follows:

[0143] 1. Define position, velocity, and acceleration as functions of time;

[0144] ;

[0145] + 4 ;

[0146] + ;

[0147] in, , , , , , These are the coefficients of six constants to be solved; For a certain axis of the robot in time Position value at time; For this axis in time Instantaneous velocity at time; For this axis in time Instantaneous acceleration at time.

[0148] 2. Solve the system of boundary equations simultaneously;

[0149] Substitute the boundary conditions, and set the total trajectory running time to T, with the starting and ending points having positions, velocities, and accelerations of 0;

[0150] ;

[0151] ;

[0152] ;

[0153] ;

[0154] 4 ;

[0155] ;

[0156] 3. Solving for coefficients;

[0157] Solve the three equations simultaneously , Get (order) );

[0158] ;

[0159] 4. The location of the trajectory points is determined;

[0160] The solution obtained arrive Substitute back to the original position function , can be used High-precision interpolation is performed at any time within the range to determine a series of end effector poses, including position, that vary along time t. And attitude parameters. After solving for the pose information using fifth-order polynomial interpolation, a second geometric smoothing is performed on the end-effector pose.

[0161] For the discrete pose points generated by the trajectory, a cubic Bézier curve is used to perform a quadratic fitting of the end position. The system receives the temporal position point sequence output from a fifth-order polynomial interpolation algorithm and determines the starting point of the path segment to be optimized. and termination point Assigning values ​​to the Bézier curve and Calculate or optimize the internal control points based on the path's velocity, acceleration, and desired minimum curvature. The path between the starting and ending points was fitted using a Bézier curve, and adjustments were made accordingly. and The position is determined until the generated curve satisfies the optimization objective of minimum curvature or shortest path. Output smooth timing position. .

[0162] ;

[0163] In the formula, u represents the parameterized distance ratio from the start point to the end point of the curve; This is the starting point, i.e., the starting position of the path; The starting control point determines the curve's position. Tangent direction and curvature at the point; To determine the termination control point, the curve is positioned at... Tangent direction and curvature at the point; The endpoint is the point where the path ends.

[0164] Then, the quaternion Slerp interpolation algorithm is combined to ensure a smooth transition of the end pose, achieving co-optimal and smooth transition of pose transformation. The starting point pose generated in the optimized trajectory is then used. and termination point attitude Convert to the corresponding quaternion .calculate The spherical angle between For each discrete time point in the trajectory Calculation time ratio Substituting these values ​​into the Slerp formula, the interpolated attitude quaternions are calculated. The interpolated result Convert back to a rotation matrix.

[0165] ;

[0166] In the formula, The interpolation pose at the time ratio; The time ratio for interpolation. (t is the current time, T is the total running time); The initial and final attitude quaternions; The angle between .

[0167] The enhanced smooth pose data generated by the above process is input into the inverse kinematics algorithm, which quickly and accurately outputs the angles of the robot's six joints at each position using analytical methods. = .

[0168] Then, the corresponding trajectory is generated based on the sequence of robot arm joint angles, as follows:

[0169] The discrete joint angle sequence obtained in the optimized trajectory generation step that satisfies fifth-order polynomial interpolation is used to optimize the trajectory generation step. As input, the position of the robot's end effector in the task space is calculated in real time using a forward kinematics algorithm. And attitude. The specific calculation process is as follows:

[0170] Using fixed DH geometry parameters and real-time joint angles As input variables, for each joint of the robot Calculate a homogeneous transformation matrix The homogeneous transformation matrix describes the first... The coordinate system relative to the first coordinate system The relative poses of the base coordinate system and the end effector coordinate system are determined by multiplying all the individual homogeneous transformation matrices sequentially to obtain the total transformation matrix from the base coordinate system to the end effector coordinate system. .

[0171] = ;

[0172] Finally, based on the final 4 Pose matrix Extract the position and orientation information of the end effector in the task space.

[0173] ;

[0174] In the formula, the position vector For the end effector in the base coordinate system ( Coordinates. Rotation matrix. The attitude of the end effector.

[0175] Connecting all discrete end-effector pose points in chronological order forms the motion trajectory of the robot's end effector. Finally, simulation software uses trajectory visualization functions to present these time-series points as a 3D motion trajectory image and a 2D time-domain trajectory plot for evaluation, such as... Figure 6 As shown.

[0176] S4. Select process points for the optimized trajectory of the six-axis robot end effector, and use a hybrid collision warning mechanism based on the separation axis theorem and neural symbol distance field to perform risk simulation verification on the selected process points until there is no interference risk in the optimized trajectory of the six-axis robot end effector, thus obtaining the final trajectory of the six-axis robot end effector, such as... Figure 5 As shown; Figure 4-5 In the diagram, X, Y, Z represent the position coordinates of the robot's end effector in the base coordinate system, with units of mm; R, P, Y represent the attitude angles of the robot's end effector in the base coordinate system; q1 q6 represents the rotation angle of the robot's six joints, in degrees of freedom (°); 6-dof represents the six degrees of freedom.

[0177] In a preferred embodiment, the process of selecting process points for the optimized trajectory of the six-axis robot end effector and using a hybrid collision warning mechanism based on the separation axis theorem and the neural symbol distance field to perform risk simulation verification on the selected process points until the optimized trajectory of the six-axis robot end effector has no interference risk, and obtaining the final trajectory of the six-axis robot end effector includes the following steps:

[0178] S41. Based on the selection method of multimodal feature point extraction and adaptive cubic spline interpolation, select several process points from the optimized trajectory of the end effector of a six-axis robot, and record the coordinates of each process point and the corresponding joint angle.

[0179] The specific implementation process of the selection method based on multimodal feature point extraction and adaptive cubic spline interpolation is as follows:

[0180] 1. Multimodal Feature Point Extraction: Multi-dimensional geometric and kinematic feature analysis is performed on the fifth-order polynomial optimized trajectory generated by S3. Geometric extrema, kinematic key points, and process constraint points are extracted from the trajectory. Geometric extrema include points with maximum curvature; kinematic key points include peaks / troughs of velocity or acceleration; and process constraint points include the closest point to an obstacle. These points constitute the initial feature point set, capable of capturing regions of dramatic changes in the trajectory's spatial morphology and dynamic characteristics.

[0181] 2. Adaptive Cubic Spline Interpolation Fitting: Using the extracted feature points as model points, an adaptive cubic spline interpolation function is constructed to locally fit the original trajectory. During the fitting process, a preset approximation error threshold is set.

[0182] 3. Process Point Iterative Selection Logic: The program automatically calculates the deviation between the interpolated trajectory and the original fifth-order polynomial trajectory. Within the deviation range exceeding a threshold, the program adaptively increases the sampling frequency based on the curvature distribution, automatically adding new feature points; while in the smooth range with smaller deviations, sparse sampling is maintained. Through this adaptive iteration, a set of discrete points that can accurately reconstruct the original trajectory features with the fewest possible values ​​is ultimately determined as the process points for subsequent collision warning verification.

[0183] S42. Using the coordinate transformation mechanism, convert the selected process points into the coordinate system of the virtual simulation model, replace the process points of the initial motion path selected in the virtual simulation model, and rerun the virtual process.

[0184] S43. In the rerun of the virtual process, the separation axis theorem is used to perform preliminary interference detection on the six-axis robot to obtain interference detection results, which include the presence of interference risk and the absence of interference risk.

[0185] In a preferred embodiment, the preliminary interference detection of the six-axis robot using the separation axis theorem during the rerun of the virtual process, and the resulting interference detection, includes the following steps:

[0186] S431. In the rerun of the virtual process, for each discrete pose in the optimized trajectory of the six-axis robot end effector, calculate the minimum bounding box of each link.

[0187] S432. Using the separation axis theorem, we can detect the intersection of the minimum bounding box of each link with the minimum bounding box of all obstacles in the environment, and obtain the detection results.

[0188] S433. Determine whether the detection result meets the preset interference conditions. If it does, determine that there is interference, identify the path segment where the interference pose is located, and merge and generate a subset of connected paths to be calculated. If it does not meet the conditions, determine that there is no interference.

[0189] Specifically, the process points selected in the trajectory planning module are converted into the coordinate system of the virtual simulation model through a coordinate transformation mechanism, replacing the process points of the initial path selected in the virtual model, and the virtual process is rerun. To improve the accuracy and efficiency of collision detection, especially in multi-robot collaborative or obstacle-dense scenarios, this invention adopts a hybrid collision warning mechanism of "Separated Axis Theorem (SAT) coarse screening and Neural Symbolic Distance Field (Neural SDF) fine calculation", which specifically includes:

[0190] First-stage coarse screening: Fast interference detection based on SAT. This method first simplifies the representation of each link of the robot and environmental obstacles using their minimum bounding box (OBB). For each discrete pose in the trajectory, the OBB of each link is calculated, and the Separating Axis Theorem (SAT) is used to check whether it intersects with the OBBs of all obstacles in the environment. SAT determines interference by judging whether the projection intervals of two OBBs on 15 candidate separating axes overlap. If the projections on any axis do not overlap, it is determined to be a no-collision scenario; if all axes overlap, it is determined to have an interference risk. The system marks the path segments where interference is detected as potentially high-risk and merges them to generate a continuous subset of paths to be refined, which serves as the input for the second-stage refined calculation.

[0191] S44. When the interference detection result indicates that there is an interference risk, the implicit collision detection method based on the neural symbol distance field is used to perform collision detection and optimize the trajectory adjustment to obtain the final trajectory of the six-axis robot end effector.

[0192] As a preferred embodiment, when the interference detection result indicates an interference risk, the implicit collision detection method based on the neural symbol distance field is used for collision detection and trajectory optimization to obtain the final six-axis robot end effector trajectory, which includes the following steps:

[0193] S441. When the interference detection result indicates that there is an interference risk, a set of sampling points is generated on the surface of each link of the six-axis robot according to a preset density based on the subset of paths to be precisely calculated.

[0194] S442. Using the principle of forward kinematics, transform the sampling point set to the global coordinate system;

[0195] S443. Construct and train the neural symbol distance field model, and input the sample point set transformed to the global coordinate system into the neural symbol distance field model to obtain the symbol distance value;

[0196] In a preferred embodiment, the construction and training of the neural symbol distance field model includes the following steps:

[0197] The point cloud data of the workstation environment is collected using a 3D scanning device, and the symbolic distance to the nearest obstacle surface point is calculated for each sampling point in the point cloud data.

[0198] The point cloud coordinates are normalized, and data augmentation is performed using random rotation, translation, and scaling to obtain augmented point cloud data.

[0199] A multilayer perceptron with position coding is used as an approximator for the symbolic distance function;

[0200] Using the enhanced point cloud data and a pre-defined composite loss function, the multilayer perceptron is trained to obtain a neural symbolic distance field model.

[0201] S444. Based on the symbol distance value, the minimum distance of all sampling points in the sampling point set is statistically analyzed. If the minimum distance of the sampling points is less than the preset safety threshold, it is determined that there is a collision risk. The path control points in the automatically adjusted and optimized trajectory are adjusted locally until the preset safety threshold is met. Otherwise, it means that there is no collision risk.

[0202] S45. When the interference detection result indicates that there is no risk of interference, the optimized trajectory of the six-axis robot end effector shall be taken as the final trajectory of the six-axis robot end effector.

[0203] It should be noted that, for the "path subset to be refined" output by the first-level coarse screening, this invention employs an implicit collision detection method based on neural symbol distance fields to achieve high-precision, differentiable distance query and collision warning. Specifically, it includes the following steps:

[0204] 1. Offline training of neural SDF models;

[0205] Data Acquisition and Labeling: High-precision point cloud data of the workstation environment was acquired using a 3D scanning device. For each sampling point x∈R3, the signed distance to the nearest obstacle surface was calculated. .

[0206] ;

[0207] in, This indicates whether the point is located inside (negative) or outside (positive) the obstacle. It is the nearest surface point.

[0208] Data preprocessing: The point cloud coordinates are normalized, and data augmentation methods such as random rotation, translation, and scaling are used to improve the model's generalization ability.

[0209] Network construction: A multilayer perceptron with positional encoding (MLP) is used as the SDF function. Approximator:

[0210] ;

[0211] in, This is a positional encoding function used to improve the network's ability to represent high-frequency geometric details.

[0212] Loss function and training: The following composite loss function is used during training. Conduct supervised learning:

[0213] ;

[0214] The first term is the mean squared error of the distance, the second term is the Eikonal constraint term, used to ensure that the learned function possesses the mathematical properties of a signed distance field; λ is the balancing weight. N represents the total number of sampling points, and i represents the index number. Indicates the first The measured pose of each sampling point This indicates the link offset.

[0215] The Adam optimizer is used for training until the loss converges, resulting in a neural SDF model that implicitly represents the geometry of the environment.

[0216] 2. Online query and collision determination;

[0217] Sampling point generation: Generate a set of sampling points on the surface of each link of the robot according to a preset density. And then transform it to the global coordinate system through forward kinematics.

[0218] Distance query: For each sampling point Input the trained neural SDF network Directly output its symbol distance value: .

[0219] Collision detection and risk warning: Statistical analysis of the minimum distance of all sampling points:

[0220] ;

[0221] like , If a preset safety threshold is not met, a collision risk is identified, triggering an early warning mechanism.

[0222] 3. Collision warning and path adjustment;

[0223] If the minimum distance calculated by the neural SDF is less than a preset safety threshold, a collision warning is triggered, and the path control points are automatically adjusted or obstacle avoidance constraints are introduced to replan the local trajectory until the safety distance requirement is met. This hybrid mechanism combines the efficiency of SAT with the high precision of neural SDF, significantly improving the accuracy and reliability of collision detection in complex and dense environments and multi-robot scenarios while ensuring real-time performance, and effectively eliminating the hidden interference risks that are difficult to detect by traditional methods.

[0224] Furthermore, once the final trajectory of the six-axis robot end effector is obtained, it indicates that there is no hidden interference risk. The running time, joint motion parameters, and other parameters of the optimized path are recorded. This invention can also perform quantitative evaluation of the optimization effect and collaborative simulation verification and performance evaluation of the final trajectory of the six-axis robot end effector.

[0225] The quantitative evaluation of optimization effects specifically includes: constructing a multi-dimensional quantitative evaluation system covering parameters such as efficiency (e.g., total trajectory running time T), smoothness (e.g., impact of each joint), and geometric optimality (e.g., total path length L). A standardization method is used to unify these indicators into dimensionless benefit-type scores. Then, a weighted scoring method is applied to calculate a weighted comprehensive score based on the assigned weights. This score is compared with a pre-set passing score to determine the effectiveness of the optimization. The specific implementation process is as follows:

[0226] 1. Multi-robot collaborative trajectory optimization;

[0227] To adapt to multi-robot collaborative operation scenarios in modern intelligent manufacturing, such as collaborative assembly, material relay handling, and multi-station collaborative welding, this invention can be extended to introduce a "distributed trajectory optimization mechanism based on communication topology perception and spatiotemporal coordination," specifically including:

[0228] 1) Multi-robot system modeling and communication topology construction;

[0229] Suppose the workstation contains N six-axis robots {R1,R2,...,R...} N Each robot possesses an independent kinematic model DH parameter set. A communication topology graph G=(V,E) is constructed between the robots, where vertices V represent robots and edges E represent communication links between robots. Depending on the actual physical layout and task coupling relationship, the communication topology can adopt star, ring, or fully connected structures to ensure that key collaborative robots have direct or indirect state awareness capabilities.

[0230] 2) Global task decomposition and local target allocation;

[0231] Based on the upper-level production tasks, the global workflow is decomposed into multiple sub-tasks and assigned to each robot. Each robot is given its own workspace, priority weight, and time window constraints, forming an initial collaborative task framework.

[0232] 3) Spatiotemporal collaborative planning strategy;

[0233] Time-based staggered access mechanism: By predicting the time windows for each robot to arrive at shared space areas, such as conveyor belt handover areas and tool sharing areas, a time occupancy map is established. An optimization algorithm based on conflict search (CBS) is employed to dynamically adjust the motion speed curves of each robot or insert controllable waiting times at key nodes, achieving staggered passage and preventing multiple robots from simultaneously competing for the same space resources.

[0234] Spatial Dimension Cooperative Obstacle Avoidance: Building upon single-robot trajectory optimization, a distributed obstacle avoidance strategy based on the velocity obstacle method (VO) or cooperative artificial potential field method is introduced. When planning its own trajectory, each robot considers not only static obstacles but also responds in real-time to the predicted trajectories of neighboring robots, adding a mutual exclusion potential energy term between robots to the potential field function.

[0235] ;

[0236] In the formula, Let be the repulsive potential energy between robot i and robot j, in joules (J) or a dimensionless value; η is the gain coefficient of the repulsive potential field, η>0, which controls the strength of the repulsive force; Let be the actual Euclidean distance between robot i and robot j; This is the preset safe distance threshold.

[0237] 3) Cooperative solution using distributed model predictive control (DMPC);

[0238] A distributed optimization architecture is adopted, where each robot independently solves for the optimal trajectory based on a local cost function containing factors such as trajectory smoothness, efficiency, and energy consumption, and collaborative constraints including safety distance and time window. Predicted trajectory information is exchanged through communication, and the process iteratively updates until a Nash equilibrium or consensus optimum is reached. The optimization problem is formulated as follows:

[0239] ;

[0240] in, These are constraints; For the control input of robot i, such as joint velocity / acceleration sequence; The state of robot i, such as position and velocity; Let N be the local cost function of robot i, which includes optimization objectives such as trajectory smoothness, efficiency, and energy consumption; ILet i be the set of neighbors of robot i, such as neighboring robots that are communicatively reachable; This represents the collaborative cost between robot i and robot j, reflecting collaborative requirements such as collision avoidance and synchronization. For robot i, single-machine constraints, such as joint limits and speed limits, etc. Establish cooperative safety constraints between robot i and robot j to ensure a minimum safe distance.

[0241] In addition, collaborative simulation verification and performance evaluation specifically include: mapping the generated collaborative trajectory set to a virtual simulation environment through a coordinate transformation mechanism to perform high-fidelity simulation of multi-robot collaborative processes. Besides single-robot evaluation metrics, a new collaborative performance evaluation dimension has been added.

[0242] System throughput: The number of tasks completed collaboratively per unit of time;

[0243] Conflict avoidance rate: The ratio of the number of potential conflicts successfully avoided to the total number of predicted conflicts;

[0244] Cooperative efficiency loss: The percentage increase in total system time due to cooperative constraints;

[0245] Communication load: The average amount of data exchanged between robots during collaborative planning;

[0246] The effectiveness, robustness, and engineering applicability of the collaborative strategy are comprehensively evaluated using multi-dimensional indicators. The corresponding process parameter design includes:

[0247] 1) DH parameter settings for a six-axis robot:

[0248] Determine the physical structure of the six-axis robot, such as joint type, link size, and joint range of motion. This requires specifying key geometric parameters such as the length of each link and joint offset. Establish a local coordinate system for each joint according to the DH standard, following the principle that "the z-axis is along the joint rotation axis, and the x-axis is along the common perpendicular of the z-axis of the two joints (from the current joint to the next adjacent joint)" to ensure that the relative position of each joint coordinate system is consistent with the robot's physical structure. Obtain the key parameters of each joint and link of the selected six-axis robot model, including joint angles, link offsets, link lengths, and link torsion angles.

[0249] 2) Trajectory planning parameter settings;

[0250] To ensure trajectory smoothness and satisfy kinematic constraints, interpolation algorithm parameters were designed to avoid motion shocks in the robot. The trajectory optimization process was set to a runtime of 2 seconds. 5s, or 2 if the path length is ≤1000mm. 3s, take 3 when the length is >1000mm A 5-second time limit is used to balance motion efficiency and smoothness, avoiding excessive acceleration due to too short a time and impacting production line cycle time due to too long a time. To ensure trajectory discretization accuracy, each optimized path has ≥50 sampling points to meet the calculation requirements of kinematic parameters such as velocity and acceleration. The velocity at the starting point of each path is set to 0 rad / s (joint velocity) and 0 mm / s (end-effector velocity), and the acceleration is set to 0 rad / s² (joint acceleration) and 0 mm / s² (end-effector acceleration) to meet the practical operational requirements of "no impact during robot start-up and no shaking during stop".

[0251] To further illustrate the feasibility of the present invention, a flexible press-fitting workstation is used as an example to optimize the path of a six-axis robot gripping a box cover.

[0252] The workstation hardware used in this experiment included: a KR210_R2700 six-axis robot, a flexible press, an automatic feeding turntable, and a manually operated turntable, etc. Simulation tools used included SolidWorks and ProcessSimulate; the trajectory optimization tool was MATLAB Robotics Toolbox.

[0253] Experimental steps:

[0254] 1) Workstation Virtual Simulation Modeling: Based on the "geometric parameter-virtual model" 1:1 mapping method, a high-precision virtual model is built using SolidWorks. ProcessSimulate is used to assign functional attributes to each device in the virtual model that are consistent with the physical entity, such as the joint motion characteristics of the robot and the opening and closing logic of the gripper. Based on the process cycle of the physical production line, the sequence of actions and interaction logic of the equipment are mapped through process replication, thereby constructing a virtual workstation simulation model. Five paths to be optimized are selected according to the simulation results.

[0255] 2) Robot Kinematic Modeling and Trajectory Planning: A laser tracker was used to measure the spatial positions of the physical KR210_R2700 robot. Based on the measurement data, parameter sensitivity analysis and error compensation were performed on the extended DH model including virtual joints to establish an individualized high-precision digital twin model of the robot for subsequent simulation. The starting and ending points of the path to be optimized were clearly defined, such as 3D spatial coordinates. A standard coordinate transformation mechanism was established based on the coordinate difference at the initial posture alignment. The coordinates of the virtual simulation model were converted into the coordinates of the trajectory planning model. Based on the coordinates of the trajectory planning model, an integrated trajectory planning algorithm that incorporates kinematic constraints was used to generate the optimized motion trajectory of the six-axis robot end effector and the corresponding 2D time domain model, such as setting a time interval of 2 seconds and 51 sampling points.

[0256] 3) Post-optimization verification: Based on the generated trajectory coordinate map, combined with multimodal feature point extraction and adaptive cubic spline interpolation algorithms, process points are selected. Coordinate points are mapped to ProcessSimulate using a coordinate transformation mechanism, replacing the initial path process points. The Separating Axis Theorem (SAT) implicit collision warning is run to finely detect interference risks between links and obstacles. If no risk is detected, the process is rerun, and the optimization effect is evaluated based on the running results.

[0257] In ProcessSimulate, the paths before and after optimization were simulated separately, and the raw data of each indicator listed in the table above were automatically recorded using the software's data logging function. The trajectory running time, root mean square acceleration of joints, total path length, and trajectory curvature smoothness of the robot's path before and after optimization were obtained. A full score standard was set for each indicator, and based on the degree of improvement of each indicator, it was mapped to the interval [0, 100] using a piecewise function. Then, weights were assigned to each indicator, and the final comprehensive score of the optimized scheme, 76.8, was calculated. A score higher than the passing score of 60 was considered an effective improvement, and the improvement effect was relatively significant.

[0258] To further verify the scalability of the invention in multi-robot collaborative scenarios, a six-axis robot of the same model KR210_R2700 was added to the flexible press-fitting workstation to construct a dual-robot collaborative handling and assembly test scenario.

[0259] Collaborative experimental steps:

[0260] 1. Collaborative Scenario Modeling: Construct a dual-robot virtual workstation in ProcessSimulate, defining the shared workspace and resource area for the two robots.

[0261] 2. Collaborative Task Definition: Robot A is responsible for picking up materials from the rack and transferring them to the transfer station, while Robot B is responsible for picking up materials from the transfer station and performing pressing operations. The two robots need to coordinate in time and space within the transfer station area.

[0262] 3. Collaborative trajectory generation: Enable the multi-robot collaborative trajectory optimization module, adopt a star communication topology, set the safe distance threshold to 150mm, and the time window tolerance to ±0.5s.

[0263] 4. Collaborative Simulation and Evaluation: Run the collaborative simulation process and record collaborative indicators such as system throughput and conflict avoidance rate.

[0264] Collaborative effect: After collaborative optimization, the dual-robot system achieved a 42% increase in throughput compared to sequential execution without physical collisions, with collaborative efficiency loss controlled within 15% and a conflict avoidance rate of 100%. This verifies the effectiveness and practicality of the invention in multi-robot collaborative scenarios.

[0265] According to another embodiment of the present invention, a six-axis robot trajectory optimization system integrating kinematic constraints and parametric simulation verification is provided, the system comprising:

[0266] The model building module is used to construct a virtual simulation model consistent with the physical scene based on the physical hardware composition, signal control logic and process cycle requirements of the workstation, and in combination with preset mapping requirements; the virtual simulation model is used to determine the path segment to be optimized for the six-axis robot.

[0267] The model building module is used to construct a trajectory planning model for a six-axis robot based on its geometric structure and kinematic constraints, and through an integrated trajectory planning algorithm that incorporates kinematic constraints.

[0268] The trajectory optimization module is used to generate the optimized trajectory of the six-axis robot end effector based on the path segment to be optimized, the virtual simulation model, and the trajectory planning model of the six-axis robot, and in conjunction with the standardized coordinate transformation mechanism, using a fifth-order polynomial interpolation algorithm.

[0269] The final trajectory determination module is used to select process points for the optimized trajectory of the six-axis robot end effector. It uses a hybrid collision warning mechanism based on the separation axis theorem and the neural symbol distance field to perform risk simulation verification on the selected process points until there is no interference risk in the optimized trajectory of the six-axis robot end effector, thus obtaining the final trajectory of the six-axis robot end effector.

[0270] 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, optical storage, etc.) containing computer-usable program code.

[0271] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., 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 method for optimizing the trajectory of a six-axis robot that integrates kinematic constraints and parametric simulation verification, characterized in that, include: S1. Based on the physical hardware composition, signal control logic, and process cycle requirements of the workstation, and combined with the preset mapping requirements, construct a virtual simulation model consistent with the physical scene through parametric modeling; use the virtual simulation model to determine the path segment to be optimized for the six-axis robot. S2. Based on the geometric structure and kinematic constraints of the six-axis robot, and through an integrated trajectory planning algorithm that incorporates kinematic constraints, a trajectory planning model for the six-axis robot is constructed. S3. Based on the path segment to be optimized for the six-axis robot, the virtual simulation model, and the trajectory planning model of the six-axis robot, and combined with the standardized coordinate transformation mechanism, the optimized trajectory of the six-axis robot end effector is generated using a fifth-order polynomial interpolation algorithm. S4. Select process points for the optimized trajectory of the six-axis robot end effector, and use the hybrid collision warning mechanism of the separation axis theorem and the neural symbol distance field to perform risk simulation verification on the selected process points until there is no interference risk in the optimized trajectory of the six-axis robot end effector, and obtain the final trajectory of the six-axis robot end effector.

2. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 1, characterized in that, The physical hardware components, signal control logic, and process cycle requirements based on the workstation, combined with preset mapping requirements, are used to construct a virtual simulation model consistent with the physical scene through parametric modeling. The process of determining the path segment to be optimized for the six-axis robot using the virtual simulation model includes the following steps: S11. Collect the geometric parameters of each device in the workstation and the motion parameters of the six-axis robot, and construct a virtual simulation model based on the mapping requirements of geometric parameters and virtual model. S12. Assign functional attributes to each device in the virtual simulation model that are consistent with the physical entity, and based on the process cycle of the physical production line, map the sequence of device actions and interaction logic through process replication and write control instructions to achieve a complete match between the virtual process and the actual production process, so as to obtain a virtual simulation model that is consistent with the physical scene. S13. Based on the operational requirements of the six-axis robot in the workstation, determine the initial motion path of the six-axis robot in the virtual simulation model; S14. Based on the virtual simulation model, the actual production process of the six-axis robot is simulated by running a virtual process, and the path segments with equipment interference and cycle mismatch problems are obtained and marked by detecting them.

3. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 1, characterized in that, The process of constructing a trajectory planning model for a six-axis robot based on its geometric structure and kinematic constraints, and through an integrated trajectory planning algorithm that incorporates kinematic constraints, includes the following steps: S21. Obtain the physical link structure of the six-axis robot, establish a local coordinate system for each link and extract the standard DH parameters; S22. Based on standard DH parameters and combined with preset virtual six-degree-of-freedom micro-motion joints, construct an extended parametric kinematic mathematical model containing compensable parameter vectors. S23. Perform global sampling in the workspace of the six-axis robot and stack the Jacobian matrix based on the sampling results. By performing spectral analysis and ill-conditioning on the Jacobian matrix, identify the key parameter combinations that cause instability in the solution of the end pose error of the six-axis robot. S24. Measure the actual end pose of multiple calibration points in the workspace of the six-axis robot, and construct a nonlinear least squares optimization model for key parameter combinations with the goal of minimizing the sum of squared residuals between the actual end pose and the theoretical pose calculated in advance by the extended parameterized kinematic mathematical model. S25. The Levenberg-Marquardt algorithm is used to iteratively solve the nonlinear least squares optimization model to obtain the enhanced DH parameter set after error compensation. S26. The enhanced DH parameter set after error compensation is used as the model parameters of the extended parametric kinematic mathematical model, and the extended parametric kinematic mathematical model is imported into the simulation software to instantiate and generate the corresponding six-axis robot trajectory planning model.

4. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 1, characterized in that, The process of generating the optimized trajectory of the six-axis robot's end effector using a fifth-order polynomial interpolation algorithm, based on the path segment to be optimized for the six-axis robot, the virtual simulation model, and the six-axis robot trajectory planning model, and in conjunction with a standardized coordinate transformation mechanism, includes the following steps: S31. Perform initial attitude alignment between the virtual simulation model and the six-axis robot trajectory planning model so that the coordinate system of the six-axis robot end effector corresponds in the three-axis direction in the virtual simulation model and the six-axis robot trajectory planning model; S32. After initial posture alignment, calculate the coordinate difference between the six-axis robot end effector in the initial posture in the virtual simulation model and the six-axis robot trajectory planning model. S33. Based on coordinate difference, construct a standardized coordinate transformation formula for converting coordinates in a six-axis robot trajectory planning model into coordinates in a virtual simulation model; S34. Determine the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for the six-axis robot, and based on the boundary conditions and total running time of the six-axis robot, use a fifth-order polynomial interpolation algorithm to determine the position of each trajectory point of the end effector of the six-axis robot, thus obtaining the optimized trajectory of the end effector of the six-axis robot.

5. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 4, characterized in that, The process of determining the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for the six-axis robot, and determining the position of each trajectory point of the six-axis robot's end effector using a fifth-order polynomial interpolation algorithm based on the boundary conditions and total running time of the six-axis robot, to obtain the optimized trajectory of the six-axis robot's end effector, includes the following steps: S341. Based on the three-dimensional spatial coordinates of the starting and ending points of the path segment to be optimized for a six-axis robot, construct a position function that includes the changes in position, velocity, and acceleration over time. S342. Based on the boundary conditions of the six-axis robot end effector and the total running time, construct a system of simultaneous boundary equations; S343. Solve the simultaneous boundary equations, substitute the solution back into the position function, and use the fifth-order polynomial interpolation algorithm to solve for the pose information of the six-axis robot end effector. S344. Connect the pose information of the six-axis robot end effector in chronological order to form the motion trajectory of the six-axis robot end effector.

6. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 5, characterized in that, The steps of solving the simultaneous boundary equations, substituting the solution back into the position function, and using a fifth-order polynomial interpolation algorithm to solve for the pose information of the six-axis robot end effector include: S3431. Substitute the solution of the simultaneous boundary equations back into the position function, and at any time within the preset time range, use the fifth-order polynomial interpolation algorithm to perform interpolation processing to obtain the discrete pose points of the six-axis robot end effector that vary along the time. S3432. For the discrete pose points of the end effector of the six-axis robot, and combined with the start and end points of the path segment to be optimized of the six-axis robot, a cubic Bézier curve is used to fit the path to generate an optimized trajectory that satisfies the minimum curvature or the shortest path. S3433. Based on the optimized trajectory that satisfies the minimum curvature or shortest path, the position and attitude information of the end effector of the six-axis robot in the task space are calculated in real time using the forward kinematics algorithm.

7. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 1, characterized in that, The process of selecting process points for optimizing the trajectory of the six-axis robot end effector and using a hybrid collision warning mechanism based on the separation axis theorem and neural symbol distance field to perform risk simulation verification on the selected process points until the optimized trajectory of the six-axis robot end effector has no interference risk, and obtaining the final trajectory of the six-axis robot end effector includes the following steps: S41. Based on the selection method of multimodal feature point extraction and adaptive cubic spline interpolation, select several process points from the optimized trajectory of the end effector of a six-axis robot, and record the coordinates of each process point and the corresponding joint angle. S42. Using the coordinate transformation mechanism, convert the selected process points into the coordinate system of the virtual simulation model, replace the process points of the initial motion path selected in the virtual simulation model, and rerun the virtual process. S43. In the rerun of the virtual process, the separation axis theorem is used to perform preliminary interference detection on the six-axis robot to obtain interference detection results, which include the presence of interference risk and the absence of interference risk. S44. When the interference detection result indicates that there is an interference risk, the implicit collision detection method based on the neural symbol distance field is used to perform collision detection and optimize the trajectory adjustment to obtain the final trajectory of the six-axis robot end effector. S45. When the interference detection result indicates that there is no risk of interference, the optimized trajectory of the six-axis robot end effector shall be taken as the final trajectory of the six-axis robot end effector.

8. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 7, characterized in that, In the process of rerunning the virtual process, the separation axis theorem is used to perform preliminary interference detection on the six-axis robot, and the interference detection results are obtained by the following steps: S431. In the rerun of the virtual process, for each discrete pose in the optimized trajectory of the six-axis robot end effector, calculate the minimum bounding box of each link. S432. Using the separation axis theorem, we can detect the intersection of the minimum bounding box of each link with the minimum bounding box of all obstacles in the environment, and obtain the detection results. S433. Determine whether the detection result meets the preset interference conditions. If it does, determine that there is interference, identify the path segment where the interference pose is located, and merge and generate a subset of connected paths to be finely calculated. If it does not meet the conditions, determine that there is no interference.

9. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 7, characterized in that, When the interference detection result indicates an interference risk, the implicit collision detection method based on the neural symbol distance field is used for collision detection and trajectory optimization to obtain the final six-axis robot end effector trajectory. This includes the following steps: S441. When the interference detection result indicates that there is an interference risk, a set of sampling points is generated on the surface of each link of the six-axis robot according to a preset density based on the subset of paths to be precisely calculated. S442. Using the principle of forward kinematics, transform the sampling point set to the global coordinate system; S443. Construct and train the neural symbol distance field model, and input the sample point set transformed to the global coordinate system into the neural symbol distance field model to obtain the symbol distance value; S444. Based on the symbol distance value, the minimum distance of all sampling points in the sampling point set is statistically analyzed. If the minimum distance of the sampling points is less than the preset safety threshold, it is determined that there is a collision risk. The path control points in the automatically adjusted and optimized trajectory are adjusted locally until the preset safety threshold is met. Otherwise, it means that there is no collision risk.

10. The six-axis robot trajectory optimization method integrating kinematic constraints and parametric simulation verification according to claim 9, characterized in that, The construction and training of the neural symbolic distance field model includes the following steps: S4431. Use a 3D scanning device to collect point cloud data of the workstation environment, and calculate the symbolic distance of each sampling point in the point cloud data to the nearest obstacle surface point. S4432. Normalize the point cloud coordinates and perform data augmentation using random rotation, translation, and scaling to obtain the augmented point cloud data; S4433, A multilayer perceptron with position coding is used as an approximator for the symbolic distance function; S4434. Using the enhanced point cloud data and a preset composite loss function, the multilayer perceptron is trained to obtain a neural symbolic distance field model.

Images