Incremental cooperative mechanical arm multi-target trajectory optimization method based on trust region and corresponding electronic equipment

Abstract

The invention discloses an incremental cooperative mechanical arm multi-target trajectory optimization method based on a trust region and corresponding electronic equipment. The incremental cooperative mechanical arm multi-target trajectory optimization method comprises the steps that S1, data are acquired; s2, establishing a model; S3, converting and solving a problem; S4, dividing a track and performing optimization execution; according to the method, the high efficiency and practicability of the cooperative mechanical arm in the complex environment are ensured through the multi-dimensional optimization model, the improved trajectory optimization method based on the trust region is adopted, and improvement is achieved in the aspects of reducing the calculation time, optimizing the trajectory length of the joint space, improving the trajectory smoothness and the like; according to the method, an incremental trajectory optimization framework is used, the environment information can be sensed in real time, and the motion information of the human body is fused into the trajectory optimization process in real time, so that the man-machine safety can be guaranteed, and the trajectory optimization quality and the calculation efficiency are effectively improved; the trajectory optimization method disclosed by the invention is superior to the prior art in processing man-machine cooperation tasks in complex scenes, and the application of the cooperation mechanical arm is perfected.

Description

Technical Field

[0001] The present invention relates to the technical field of collaborative robot arms, and specifically to an incremental trust region-based collaborative robot arm multi-objective trajectory optimization method and corresponding electronic equipment. Background Art

[0002] Collaborative robotic arms are a type of robot designed to interact directly with humans in a shared workspace. They are designed to work safely and efficiently with human workers. Unlike traditional industrial robots, collaborative robotic arms can work with humans in dynamic environments. They have greater flexibility, adaptability and interoperability, and can be widely used in manufacturing, logistics, medical and other fields to perform a variety of tasks such as assembly, handling, and packaging. Through sensors and control algorithms, collaborative robotic arms can perceive the surrounding environment in real time, avoid collisions with people, and ensure safety.

[0003] On the technical level, collaborative robot arm trajectory optimization is a key technology. By optimizing the trajectory of the robot arm, the working efficiency and accuracy of the robot arm under different working conditions can be improved. Compared with traditional industrial robots, the trajectory planning of collaborative robot arms not only needs to consider the robot arm's own motion performance and traditional working conditions, but also needs to consider the constantly changing external environment and personnel interaction to achieve the expected working results, which puts higher requirements on trajectory optimization. For example, in complex assembly tasks, collaborative robot arms need to consider not only the position, posture and other information of parts, but also dynamic obstacles, trajectory end points and other information to avoid collisions with workers while performing assembly operations.

[0004] However, when dealing with multi-objective problems, traditional trajectory optimization methods often suffer from slow convergence and poor real-time performance due to high computational complexity. To solve this problem, it is necessary to introduce more efficient optimization algorithms and technologies so that collaborative robotic arms can achieve fast and accurate trajectory adjustments under a variety of working conditions to achieve the expected working results. Summary of the invention

[0005] The purpose of the present invention is to provide an incremental trust region-based collaborative robot arm multi-objective trajectory optimization method and corresponding electronic equipment that can improve the collaborative robot arm's ability to handle complex problems such as nonlinearity and multi-objective optimization.

[0006] Based on the above purpose, the present invention adopts the following technical solution:

[0007] An incremental trust region-based robot multi-objective trajectory optimization method comprises the following steps:

[0008] S1. Obtain data: Create a topic communication in the Robot Operating System (ROS) to read joint_states to obtain the state vector of the robot in the joint space, including joint angle, angular velocity, and angular acceleration; obtain the position of the robot's end effector in the workspace through the Transform Library function package in ROS; use the Nokov Mars motion capture device to collect data on the object installed at the target point and obtain environmental data in real time and human motion trajectory

[0009] S2, establishing a model: establishing a multi-objective, multi-constraint trajectory optimization model based on the data obtained in step S1;

[0010] S3, Problem transformation and solution: The nonlinear constraints of the trajectory optimization problem are transformed into penalty functions through the trust region-based optimization method, and the non-convex optimization problem is iteratively solved through a series of approximate problems;

[0011] S4, trajectory division and optimization execution: The original trajectory is divided into coarse trajectory clusters through the incremental trajectory optimization framework, the environmental information is input at a certain frequency, the trajectory is optimized, and the feasible trajectory of the corresponding time period is executed.

[0012] Preferably, the specific process of establishing the model in step S2 includes:

[0013] Determine multiple objective functions for trajectory optimization, including path length, energy consumption, obstacle avoidance, etc.; set multiple constraints for trajectory optimization, including speed constraints, acceleration constraints, joint angle constraints, etc.; construct a mathematical model of objective functions and constraints, and express them in the form of optimization problems to form a multi-objective, multi-constraint optimization problem:

[0014]

[0015] subject to g i (ξ)≤0,i=1,2,...,n ineq

[0016] h i (ξ)=0,i=1,2,...,n eq

[0017] In the formula, is the objective function, ω i is the weight coefficient, g i (ξ)≤0,i=1,2,...,n ineq is an inequality constraint, where n ineq is the number of inequality constraints, hi (ξ)=0,i=1,2,...,n eq is an equality constraint, where n eq is the number of equality constraints.

[0018] Preferably, the specific process of problem transformation and solution in step S3 includes:

[0019] S3.1, Constraint transformation: Use penalty function method to deal with nonlinear constraints and transform the constraints into part of the objective function;

[0020] S3.2, Optimization problem transformation: Construct trust region subproblems to transform the original non-convex optimization problem into a series of approximate convex optimization problems;

[0021] S3.3. Solve the optimal solution: gradually approach the optimal solution by iteratively solving the trust region subproblem.

[0022] Preferably, the specific process of the constraint condition transformation in step S3.1 includes:

[0023] Determine the constraints and express them in the form of equations or inequalities; introduce the constraints into the objective function and construct a new objective function, that is, add the penalty function term to the original objective function; select appropriate penalty function parameters (penalty factors) to determine whether the optimization result meets the constraints. If not, continuously increase the penalty factor to ensure that the penalty function can effectively constrain the optimization process; the form of the optimization problem after constraint transformation is:

[0024]

[0025] subject tog lin,i (ξ)≤0,i=1,2,...,n lin_ineq

[0026] h lin,i (ξ)=0,i=1,2,...,n lin_eq

[0027] Where σ is the penalty factor; the inequality constraint g lin,i (ξ)≤0 is transformed into |g in the penalty objective function nolin,i (ξ)| + ,|x| + =max(x,0), where n lin_ineq is the number of inequality constraints, n nolin_ineq is the number of inequality constraints after transformation; the equality constraint h nolin,i (ξ)=0 is converted into |h in the penalty objective function nolin,i (ξ)|, where n lin_eq is the number of equality constraints, n nolin_eqis the number of equality constraints after transformation.

[0028] Preferably, the specific process of optimizing the problem transformation in step S3.2 includes:

[0029] Based on the current solution, a trust region is defined, that is, the local search area of ​​the solution; according to Taylor expansion, an approximate model is constructed in the trust region, which can be a linear or quadratic model; the approximate model is used to replace the original problem and the optimization solution is performed in the trust region.

[0030] Preferably, the specific process of solving the optimal solution in step S3.3 includes:

[0031] In each iteration step, the approximate model is used to solve the subproblem and obtain a new solution; check whether the new solution meets the requirements of the trust region. If so, update the trust region and the current solution; otherwise, reduce the trust region and continue to solve the subproblem until a solution that meets the requirements is found; in each iteration process, by adjusting the size and position of the trust region, the search range is gradually narrowed, the solution is continuously updated, and the global optimal solution is gradually approached; through multiple iterations, it is ensured that the final solution is optimal in the global scope.

[0032] Preferably, the specific process of dividing the trajectory and optimizing the execution in step S4 includes:

[0033] S4.1, Divide coarse trajectory clusters: Initial trajectory generation, divide the original trajectory into coarse trajectory clusters;

[0034] S4.2, optimize the coarse trajectory cluster: incremental optimization, gradually input the real-time environment information, and perform local optimization on the coarse trajectory cluster;

[0035] S4.3. Execute feasible trajectories: Execute the optimized trajectory clusters in sequence according to time periods to ensure the continuity and safety of the robot's motion.

[0036] Preferably, the specific process of dividing the coarse trajectory clusters in step S4.1 includes:

[0037] According to the length and complexity of the initial trajectory, it is divided into several segments to form a coarse trajectory cluster;

[0038] Each rough trajectory contains a series of trajectory points, which represent the spatiotemporal information of the robot's running trajectory, and are represented by the control vector of the robot's configuration space and the time interval between the trajectory points.

[0039] Preferably, the specific process of optimizing the rough trajectory cluster in step S4.2 includes:

[0040] The current environmental information is obtained through sensors, and the internal parameters of the robot trajectory optimizer are updated according to the robot trajectory executed in the previous optimization cycle, the observed human trajectory and the current environmental state; according to the current human motion state, the human motion trajectory is predicted through the uniform motion model to obtain the predicted trajectory of the human body in the next optimization cycle; the optimizer optimizes the trajectory according to the set goals and constraints, using an improved trajectory optimization method based on trust region to generate an optimized trajectory.

[0041] An electronic device includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, any step in any of the above-mentioned incremental trust region-based robot multi-objective trajectory optimization methods is implemented.

[0042] The beneficial effects of the present invention include:

[0043] The present invention comprehensively considers multiple objective functions in the trajectory optimization model, including motion efficiency, path smoothness and energy consumption, and simultaneously processes multiple constraint conditions, including safety distance, workspace limitations and dynamic obstacles, and uses the entropy weight method to determine the weight configuration of each optimization objective in the model; through the multi-dimensional optimization model, the efficiency and practicality of the collaborative robot arm in complex environments are ensured, so that the generated trajectory not only meets the technical requirements, but also can adapt to the complex environment and changeable situations in practical applications.

[0044] The present invention adopts an improved trust region-based trajectory optimization method, and optimizes the processing of failed iteration points by introducing a trust region-linear search algorithm, thereby improving the computational efficiency; further, the present invention adopts a Gaussian mixture model to capture the changing relationship between the penalty factor and the degree of constraint violation, so as to optimize the iterative process of the penalty factor, and effectively improves the iterative stopping phenomenon of the penalty factor through an iterative strategy based on safety distance adjustment; in typical collision avoidance scenarios, the present invention has improvements and enhancements in reducing the computational time, optimizing the trajectory length of the joint space, and improving the trajectory smoothness, and is particularly suitable for complex scenarios.

[0045] The incremental trajectory optimization framework used in the present invention combines improved trust and trajectory optimization methods, and integrates the motion information of the human body into the trajectory optimization process in real time, which can not only ensure the safety of humans and machines, but also effectively improve the optimization quality and computational efficiency of the trajectory; through trajectory planning safety and efficiency experiments, it is verified that the method of the present invention meets the needs of collaborative robot arm trajectory planning, and compared with the commonly used real-time trajectory optimization methods, the method of the present invention has obvious advantages in trajectory efficiency and quality, and has good applicability to different scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1This is a flow chart of a multi-objective trajectory optimization method for a collaborative robot arm according to Embodiment 1 of the present invention;

[0047] Figure 2 It is a flow chart of an improved trajectory method based on trust region according to Embodiment 1 of the present invention;

[0048] Figure 3 This is a pseudo code diagram of an improved trust region algorithm according to Embodiment 1 of the present invention;

[0049] Figure 4 This is a comparison chart of the optimization trajectory evaluation results of the method of the present invention, TrajOpt, and the potential field method in Example 2 of the present invention;

[0050] Figure 5 is a comparison chart of the average calculation time between the method of the present invention and other methods in Example 2 of the present invention;

[0051] Figure 6 is a comparison diagram of the average trajectory length between the method of the present invention and various methods in Example 2 of the present invention;

[0052] Figure 7 is a comparison diagram of the method of the present invention and other methods in terms of average angular acceleration in Example 2 of the present invention;

[0053] Figure 8 This is a schematic diagram of scene settings of Embodiment 3 of the present invention;

[0054] Fig. 9 This is a schematic diagram of a static obstacle avoidance scenario according to Embodiment 3 of the present invention;

[0055] Fig.10 This is a schematic diagram of a dynamic obstacle avoidance scenario according to Embodiment 3 of the present invention;

[0056] Fig.11 This is a graph of robot arm trajectory data for a static obstacle avoidance scenario according to Embodiment 3 of the present invention;

[0057] Fig.12 This is a diagram of the trajectory data of the robot arm in the dynamic obstacle avoidance scenario of Example 3 of the present invention;

[0058] Fig.13 This is a static obstacle scene robot arm trajectory data diagram of Example 3 of the present invention;

[0059] Fig.14 This is a diagram of the trajectory data of the robot arm in a dynamic obstacle scene according to Embodiment 3 of the present invention;

[0060] Fig.15 This is a long-distance curve diagram in a static obstacle scene of Example 3 of the present invention;

[0061] Fig.16This is a long-distance curve diagram in a dynamic obstacle scene of Example 3 of the present invention;

[0062] Fig.17 This is a layout diagram of a collaborative assembly workbench according to Embodiment 4 of the present invention. DETAILED DESCRIPTION

[0063] Example 1

[0064] The following is a further explanation of the present invention in conjunction with specific embodiments.

[0065] like Figure 1 As shown, this embodiment is an incremental trust region-based collaborative robot arm multi-objective trajectory optimization method, comprising the following steps:

[0066] S1. Get data: Create a topic communication in the Robot Operating System (ROS) to read joint_states to obtain the state vector of the robot in the joint space, including the joint angle θ, angular velocity and angular acceleration The Transform Library function package in ROS is used to obtain the position of the robot's end effector in the workspace; the Nokov Mars motion capture device is used to collect data on the objects installed at the target point to obtain environmental data in real time. and human motion trajectory

[0067] S2. Model building: Building a multi-objective, multi-constrained trajectory optimization model, including:

[0068] S2.1. Determine multiple objective functions for trajectory optimization, including: constructing a model for motion efficiency; a model for minimizing the robot's motion path; a model for trajectory smoothness; a model for task completion rate; and a model for human-machine safety.

[0069] S2.2, set multiple constraints for trajectory optimization, including speed constraints, acceleration constraints, joint angle constraints, etc.;

[0070] S2.3. Construct a mathematical model of the objective function and constraints, and express it in the form of an optimization problem to form a multi-objective and multi-constraint optimization problem.

[0071] Specifically,

[0072] S2.1. Construct multiple objective functions for trajectory optimization, including:

[0073] Construct a motion efficiency model, including: optimizing the acceleration of the joint angle to reduce the high-frequency vibration of the robot arm during movement, as well as the energy consumption of movement. The objective function is:

[0074]

[0075] Where, the optimization variable ξ is an N×K dimensional vector, which is the robot trajectory represents the trajectory of the robot at the Tth optimization cycle, where N is the time step and K is the dimension of the control input; θ t is the state vector of the robot in the joint space at the tth moment, where θ∈R N .

[0076] Construct a model that minimizes the robot's motion path, and the objective function is:

[0077]

[0078] In the formula, θ t+1 is the state vector of the robot in the joint space at the t+1th moment.

[0079] Construct a trajectory smoothing model, including minimizing the change in the robot's end position and acceleration. The objective function is:

[0080]

[0081] In the formula, is the position of the robot end effector in the workspace at the tth moment; is the position of the robot end effector in the workspace at the t+1th moment; is the acceleration of the robot end effector in the workspace at the tth moment.

[0082] Construct a model for task completion rate, including optimizing the index by minimizing the deviation between the robot end motion position under the nominal trajectory and the robot end motion position under the optimized trajectory. The objective function is:

[0083]

[0084] In the formula, It represents the end position of the robot under the nominal trajectory at time t.

[0085] Construct a human-machine safety model, including: the robot avoids conflicts or collisions with human collaborators or the surrounding environment; the robot moves within an appropriate working range, with the following constraints:

[0086]

[0087] In the formula, represents the set of robot links at time t, represents the set of obstacles at time t. j} represents the set of hyperplanes that limit the working range. sd(A,B) represents the calculation of the directed distance between object A and object B. links is the number of connecting rods of the robot arm, N obstacles is the number of obstacles, N hyperplane The number of hyperplanes that limit the working range.

[0088] S2.2. Set multiple constraints for trajectory optimization, set angle limits, speed and acceleration limits for each joint of the robot, and add dynamic constraints. The specific constraint forms are as follows:

[0089] θ min ≤θ≤θ max

[0090]

[0091] Where θ is the robot joint angle, is the robot angular velocity, is the robot angular acceleration.

[0092] S2.3, the form of establishing multi-objective and multi-constraint optimization problem is:

[0093]

[0094] subject to g i (ξ)≤0,i=1,2,...,n ineq

[0095] h i (ξ)=0,i=1,2,...,n eq

[0096] In the formula, is the objective function, ω i is the weight coefficient, g i (ξ)≤0,i=1,2,...,n ineq is the inequality constraint, h i (ξ)=0,i=1,2,...,n eq is an equality constraint.

[0097] S3. Problem transformation and solution: The nonlinear constraints of the trajectory optimization problem are transformed into penalty functions through the trust region-based optimization method, and the non-convex optimization problem is iteratively solved through a series of approximate problems, such as Figure 2 As shown, the following steps are included:

[0098] S3.1. Constraint transformation: Initialize the nominal trajectory and transform the optimization problem through the first-order penalty:

[0099] First, initialize the nominal trajectory ξ (0) :From the rough planning trajectory cluster Select the rough planning trajectory of the current optimization cycle As the nominal trajectory for initialization.

[0100] Then determine the constraints: each joint of the robot has angle limits, speed and acceleration upper limits, and add dynamic constraints to express them in the form of equations or inequalities.

[0101] The optimization problem is transformed through first-order penalty: the various nonlinear constraints are transformed into the objective function with penalty factor by using the exterior point penalty function method; the penalty factor σ is continuously increased in the loop to ensure that the final feasible solution must satisfy all constraints; the optimization problem after constraint transformation is in the form of:

[0102]

[0103] subject tog lin,i (ξ)≤0,i=1,2,...,n lin_ineq

[0104] h lin,i (ξ)=0,i=1,2,...,n lin_eq

[0105] Where σ is the penalty factor; the inequality constraint g nolin,i (ξ)≤0 is transformed into |g in the penalty objective function nolin,i (ξ)| + ,|x| + =max(x,0); equality constraint h nolin,i (ξ)=0 is converted into |h in the penalty objective function nolin,i (ξ)|.

[0106] S3.2. Optimization problem transformation: Construct trust region subproblems to transform the original non-convex optimization problem into a series of approximate convex optimization problems.

[0107] like Figure 3 As shown, the improved trust region algorithm is used to solve the transformed optimization problem: at the current iteration point x (k) Construct a local approximation model near to approximate the original problem, which is used to approximate the local approximation function of the objective function Usually a quadratic function; use this local approximation function at x (k) The minimum point in a neighborhood (trust region) of is taken as the next iteration point x (k+1) .

[0108] S3.3. Solve the optimal solution: gradually approach the optimal solution by iteratively solving the trust region subproblem.

[0109] In each iteration, the approximate model is used to solve the subproblem and obtain a new solution. Check whether the new solution meets the requirements of the trust region. If so, update the trust region and the current solution. Otherwise, reduce the trust region and continue to solve the subproblem until the iteration point x (k) When the convergence conditions are met, the algorithm iteration cycle ends and the optimal solution is output.

[0110] S4, dividing trajectories and optimizing execution: The original trajectory is divided into coarse trajectory clusters through the incremental trajectory optimization framework, environmental information is input at a certain frequency, the trajectory is optimized, and the feasible trajectory of the corresponding time period is executed. The specific process includes:

[0111] S4.1. Divide the coarse trajectory clusters: Initial trajectory generation, divide the original trajectory into coarse trajectory clusters:

[0112] Obtain the nominal trajectory: According to the set starting position of the robot motion Target pose Get the nominal trajectory The initialization process of the nominal trajectory includes but is not limited to linear interpolation, existing path planning methods, and generation of a trajectory library specific to application scenarios. No requirements are made for motion collision avoidance of the nominal trajectory.

[0113] According to the set planning step Step plan , and execution step length Step execute , which is divided into clusters of rough planning trajectories According to the set planning step Step plan , and execution step length Step execute , which is divided into clusters of rough planning trajectories Each rough trajectory contains a series of trajectory points, which represent the spatiotemporal information of the robot's running trajectory. The control vector of the robot's configuration space and the time interval between the trajectory points are represented by each rough trajectory. The length of Step plan , the distance between the starting points of two adjacent trajectories is Step execute .

[0114] S4.2, optimize the coarse trajectory cluster: incremental optimization, gradually input the real-time environment information, and perform local optimization on the coarse trajectory cluster;

[0115] After the rough trajectory is generated, the optimization loop is entered; in the optimization loop, the planner calculates the robot trajectory according to the previous optimization cycle. Observed human trajectories And the current state of the environment Updates the internal parameters of the robot trajectory optimizer, which affects the predictability and readability of the generated trajectory.

[0116] According to the current state of human movement The motion trajectory of the human body is predicted by the uniform motion model to obtain the predicted trajectory of the human body in a future optimization cycle:

[0117]

[0118] The optimizer optimizes the trajectory according to the set multi-objectives and multi-constraints using an improved trajectory optimization method based on trust region to generate an optimized trajectory that meets the requirements of the collaborative task.

[0119] S4.3. Execute feasible trajectories: Execute the optimized trajectory clusters in sequence according to time periods to ensure the continuity and safety of the robot's motion.

[0120] First, the planner executes the robot's execution trajectory Optimizing trajectories for robots Previous Step execute Length of the trajectory segment.

[0121] Then determine whether the target point has been reached If you reach the destination The current plan ends; if the target point is not reached Then replace the part of the coarse trajectory cluster that corresponds to the optimized trajectory at the time point, and replace the original trajectory points with the same trajectory points in the optimized trajectory, and update the coarse trajectory cluster to And repeat the above optimization cycle until the target point is reached

[0122] Example 2

[0123] This embodiment conducts simulation experiments on the method in Embodiment 1 and multiple existing methods and compares the experimental results:

[0124] The method in Example 1 was compared with TrajOpt and the potential field method in static and dynamic obstacle scenarios, and the results are as follows: Figure 4 As shown; in the two scenarios, the method in Example 1 is significantly better than TrajOpt in terms of normalized total calculation time and nominal trajectory tracking accuracy, slightly better than TrajOpt in terms of normalized trajectory smoothness and comprehensive trajectory length ratio, and slightly weaker than TrajOpt in terms of safety. This is because the segmented optimization and linear search modules of the incremental trajectory optimization make its optimization process more refined and the optimization results better, but it also causes the trajectory points to be relatively close to obstacles.

[0125] In both scenarios, the patented method is superior to the potential field method in terms of comprehensive trajectory length ratio, safety, and normalized trajectory smoothness, but is weaker than the potential field method in terms of normalized total calculation time and normalized nominal trajectory tracking accuracy. Due to the low computational complexity of the potential field method, it has a great advantage in computational efficiency, but the patented method is not much different from it in the static obstacle scenario, which indirectly confirms that it also has high computational efficiency.

[0126] The method in Example 1 was compared with three trajectory optimization methods based on trust region-linear search and three trajectory planning methods based on optimization in three preset scenarios. Figure 5-7 As shown; the three scenarios include:

[0127] 1. Scenario 1: This simulates the situation where the operator's forearm directly intrudes into the robot's nominal trajectory, representing a general collision avoidance test environment.

[0128] 2. Scenario 2: This scenario simulates the situation in which the operating arm and the robot's nominal trajectory partially overlap during collaborative work, resulting in a large section of continuous trajectory points being infeasible. This scenario is mainly used to test the trajectory planning algorithm's ability to replan when encountering a large range of spatial constraints, as well as to take into account trajectory continuity while ensuring human-machine safety.

[0129] 3. Scenario 3: simulates the interference between the operator's arm and the robot's nominal trajectory; the characteristic of this scenario is that although there are relatively few interfering trajectory points, in order to avoid such interference, the required trajectory adjustment range is large, and the algorithm needs to have good global optimization capabilities.

[0130] Through the above simulation experiments, it can be seen that the evaluation results of the calculation time of the patented method (TR-LS-Break-GMM) in scene two and scene three are 0.130s and 0.297s respectively, and the evaluation results of the trajectory smoothness are 1.243rad / s2 and 1.086rad / s2 respectively, which are better than the existing methods (CHOMP, STOMP, TrajOpt), showing that it is superior to the existing methods in terms of computational efficiency and smoothness of the generated trajectory.

[0131] However, in scenario one, the method is better than the TrajOpt method in terms of computational time (0.345s) and trajectory length (5.098rad, 5.093rad, 5.117rad) in all scenarios, but lags behind the CHOMP and STOMP methods. This is mainly because the solution of scenario one is simpler than scenarios two and three, and methods such as CHOMP and STOMP that use soft constraints can converge faster, thus having higher computational efficiency and shorter trajectory length. In contrast, TrajOpt and the patented method pay more attention to constraint processing and comprehensive consideration of multiple objectives, but the problem to be solved in a single iteration is more complex, so the advantages in computational efficiency and trajectory length are not obvious in simpler scenarios, but they perform better in trajectory smoothing.

[0132] The robot motion intentions of the method in Example 1, the prediction error minimization method, the terminal path minimization method, and the joint path minimization method were evaluated subjectively and objectively, and the results are shown in Tables 1 and 2:

[0133] Table 1 Subjective evaluation results of robot motion intention

[0134]

[0135]

[0136] As can be seen from Table 1, the evaluation results of the method proposed in this patent in terms of fluency, readability, predictability and naturalness are 5.58, 6.05, 6.00 and 5.73 respectively, which has obvious advantages over the joint path minimization method and the terminal path minimization method.

[0137] In addition, the evaluation results of the prediction error minimization method in readability, predictability and naturalness are 5.75, 5.58 and 5.50 respectively. Compared with them, the method of this patent has more advantages in these three indicators; in terms of fluency, the evaluation result of the prediction error minimization method is 5.70, which performs best, followed by the method proposed in this patent, which has advantages over the joint path minimization method and the terminal path minimization method; the subjective evaluation results of the robot's motion intention show that compared with the existing methods, the method proposed in this patent has advantages in all subjective evaluation indicators, and is only slightly weaker than the prediction error minimization method in terms of fluency.

[0138] Table 2 Objective evaluation results of robot motion intention

[0139]

[0140] As can be seen from Table 2, the method proposed in this patent performs well in various indicators: the remaining time ratio is 0.36, the correct time is 2.18s, the total error is 0.28m, and the intention recognition time is 0.94s; the results of objective evaluation further verify that compared with the existing methods, the method proposed in this patent has better performance in expressing robot motion intentions.

[0141] In summary, the present invention discloses an incremental trust region-based collaborative robot arm multi-objective trajectory optimization method, which consists of a multi-objective, multi-constrained trajectory optimization model, a trust region-linear search algorithm and an incremental trajectory optimization framework; by establishing a multi-objective, multi-constrained trajectory optimization model and using the entropy weight method to configure the optimization target weights, the nonlinear constraints of the trajectory optimization problem are converted into penalty functions through a trust region-based optimization method, and the non-convex optimization problem is iteratively solved through a series of approximate problems; the original trajectory is divided into coarse trajectory clusters through an incremental trajectory optimization framework, environmental information is input at a certain frequency, the trajectory is optimized, and the feasible trajectory of the corresponding time period is executed; the computational efficiency and trajectory smoothness are significantly improved; the method integrates human motion information in real time, ensures the safety of human-machine collaboration, and adapts to the rapidly changing needs in a dynamic working environment; taking all aspects into consideration, the method of the present invention shows advantages over the prior art in processing human-machine collaboration tasks in complex scenarios, and provides support for the practical application of collaborative robot arms.

[0142] Example 3

[0143] In this embodiment, a test scenario is designed to simulate the human-computer interaction that may be encountered in actual assembly, and the method in Embodiment 1 is tested on a real machine; the scenario is set as follows Figure 8 As shown in the figure, the collaborative robot arm adopts FANUC CR-7iA, the human body collision model mainly considers the arm part, and capsule models H1 and H2 are established through the three target points of the wrist, elbow and shoulder; the collaborative robot arm collision model mainly considers the robot arm segment, joints, fixtures, etc., and the capsule models R1, R2, R3, and R4 are determined by the connecting rod length and connecting rod offset of each part of the robot arm, among which the main axis length of the fourth capsule is extended to cover the end tool.

[0144] like Fig. 9 and Fig.10 In this example, two typical test scenarios are designed to simulate the human-computer interaction situations that may be encountered in assembly, including:

[0145] 1. Static obstacle avoidance scenario: Humans appear as static obstacles in the robot's original planned trajectory, simulating the situation where personnel need to install or adjust assembly parts at specific locations in a shared space between humans and machines. The robot's initial terminal trajectory consists of two straight lines, and the nominal trajectory of the experiment is obtained by interpolation.

[0146] 2. Dynamic obstacle avoidance scenario: A human passes through the robot's original planned trajectory, simulating the situation where a person carries out the handling of small parts and tools in a shared space between humans and the robot. The nominal trajectory is the same as the static scenario.

[0147] The results of the test run are as follows Fig.11 and Fig.12 As shown in the figure, the yellow line represents the motion trajectory of the robot end, and the red line represents the motion trajectory of the human arm end; in the test of this embodiment, the following points of the method in embodiment 1 are mainly evaluated:

[0148] Trajectory smoothness: The trajectory data of the robot arm in the static obstacle scene test is as follows: Fig.13 As shown in Figure 2, the trajectory data of the robot arm in the dynamic obstacle scene test is as follows: Fig.14 As shown in the figure, in the static obstacle scenario, the maximum angular acceleration of the robot arm is 2.01rad / s2, and the maximum angular jerk is 13.41rad / s3; in the dynamic obstacle scenario, the maximum angular acceleration of the robot arm is 3.24rad / s2, and the maximum angular jerk is 21.57rad / s3, which meets the trajectory smoothness requirements.

[0149] Movement efficiency: Movement efficiency is measured by the ratio of the trajectory length between the optimized trajectory and the shortest possible trajectory under ideal conditions; the trajectory length in joint space is calculated as:

[0150]

[0151] In the static obstacle scenario, the ideal shortest trajectory length is 3.45 rad, the optimized trajectory length is 3.92 rad, and the trajectory length ratio is 1.14. In the dynamic obstacle scenario, the ideal shortest trajectory length is 3.45 rad, the optimized trajectory length is 4.97 rad, and the trajectory length ratio is 1.44, which meets the motion efficiency requirements.

[0152] Real-time trajectory replanning: The incremental trajectory optimization framework needs to replan the current trajectory segment in each optimization cycle; in the static obstacle scenario, the total calculation time of the trajectory optimization method is 177ms, the average replanning time is 4.36ms, and the longest replanning time is 17.54ms, which meets the real-time trajectory replanning requirements established in Section 2.1; in the dynamic obstacle scenario, the total calculation time of the trajectory optimization method is 566ms, the average replanning time is 11.32ms, and the longest replanning time is 21.27ms, which meets the real-time trajectory replanning requirements.

[0153] Arrival accuracy: The teach pendant reads the terminal posture of the FANUC robot when it moves to the end point under the nominal trajectory, static obstacle scene and dynamic obstacle scene. The position error does not exceed ±0.1mm, and the posture error does not exceed ±1°, which meets the arrival accuracy requirements.

[0154] Safety margin: During the robot's motion, the distance between each capsule model of the robot and the obstacle changes as shown in the following figure: Fig.15 and Fig.16 As shown; as shown in the figure, the method of embodiment 1 of the present invention meets the safety margin requirements in both static and dynamic obstacle scenarios.

[0155] Example 4

[0156] This embodiment uses the method in Embodiment 1 to control a collaborative robotic arm to conduct a computer motherboard collaborative assembly experiment; the assembly parts involved in the computer motherboard assembly experiment include a motherboard, a CPU, and a CPU radiator, and the parts connection forms include two common forms: threaded connection and snap connection; the motherboard and the CPU are connected by snaps, and the motherboard and the CPU radiator are fixed by screws; the assembly of the computer motherboard requires the installation of the motherboard and the CPU first, then the installation of the CPU radiator, and finally the transportation of the computer motherboard; the actions of the CPU installation and the CPU radiator screw tightening are relatively complex and delicate, and therefore are manually operated; the transfer of tools and the transportation of various components are performed by collaborative robots.

[0157] The workbench layout for collaborative assembly is as follows Fig.17 As shown, assembly point 1 is the initial position, and the mainboard can collaboratively install the CPU at assembly points 1 and 2; assembly point 2 is used to collaboratively install the CPU radiator on the mainboard. Since the installation of this component requires screw tightening operations, assembly point 2 is equipped with a support to connect with the CPU radiator installation part on the mainboard; after the installation of the finished product is completed, the collaborative robot transports it to the finished product handover point, and the task is completed.

[0158] A finite state machine is constructed for the computer motherboard assembly task, and the assembly process tasks of the collaborative robot are planned through each collaborative state and the transfer and actions between states.

[0159] The states of collaborative tasks are mainly divided into three categories: grabbing and placing, tool delivery, and finished product handling. The states of grabbing and placing include initial position (START_POS), grabbing CPU radiator (PICK_CPUFAN), grabbing motherboard (PICK_CPUFAN), and grabbing

[0160] (PICK_MOTHERBOARD), place CPU heat sink (PLACE_CPUFAN), place motherboard (PLACE_MOTHERBOARD); the status of tool delivery includes grabbing tool (GRAB_TOOL), handing over tool (HAND_OVER_TOOL), placing tool (PLACE_TOOL); the status of finished product transportation includes grabbing finished product (GRAB_OBJ), transporting finished product to transfer point (TRANSPORT_OBJ); the state transition of the state machine is carried out through the completion of the action corresponding to the current state and the operation status of the current collaborative personnel.

[0161] The actions of the state machine are the trajectory planning and execution of the robot. The actions of grabbing and placing include moving to the initial position, grabbing action (including two grabbing actions: moving to assembly point 1 and grabbing the motherboard, moving to the parts area and grabbing the CPU cooler), and placing action (including two placing actions: moving to assembly point 2 and placing the motherboard, moving to the assembly waiting area and placing the CPU cooler); the action of handing over tools includes grabbing tools, handing over tools, and putting the tools back in place; the action of handling finished products includes moving to assembly point 2 and grabbing finished products, moving to the handover point and placing finished products.

[0162] The triggering conditions of the state machine include completion of the action corresponding to the state (done), CPU heat sink not picked up (CPU_no_pick), mainboard not picked up (MB_no_pick), CPU installation by the collaborator (human_operating_CPU), CPU installation completed (CPU_task_finished), CPU heat sink installation by the collaborator (human_operating_CPUFAN), CPU heat sink installation completed (CPUFAN_task_finished), part pick-and-place completion (pick_place_finished), and component installation task completion (assembly_task_finished); the progress and completion of the installation operation by the collaborator are judged by the distance between the hand and the assembly point; the release and gripping timing of the robot end gripper during the tool handover process are judged by the distance and dwell time between the tool and the end.

[0163] The operator and the robot perform a computer motherboard assembly task in a collaborative environment. The motion capture system collects human motion data and environmental data, and sends them online to the collaborative robot planning platform. The finite state machine of the collaborative assembly task planning module determines the current collaborative task state of the robot, and the nominal trajectory of the task is obtained through the nominal trajectory generation module; the incremental trajectory optimization module uses the method in Example 1 of the present invention, combined with the input of the human motion prediction module and the collision detection module, to optimize the robot motion trajectory in real time, and sends motion instructions to the collaborative robot for execution through the collaborative robot control middleware.

[0164] This collaborative assembly case verifies the effectiveness and practicality of the incremental trajectory optimization method and human-computer interaction strategy in Example 1 of the present invention, and demonstrates the application potential of the collaborative assembly system; when humans and machines share a workspace, this method can effectively identify and respond to dynamic changes in the environment, ensuring smooth operation and the adaptability of the robot.

[0165] The above description is only a further explanation of the present invention in combination with specific embodiments. All descriptions do not limit the scope of protection of the present invention. Any changes or replacement schemes that can be easily thought of by any technician in this field within the technical scope disclosed by the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. An incremental trust region-based robot multi-objective trajectory optimization method, characterized in that: The following steps are involved: S1. Data acquisition: The robot's state vector in the joint space and the position of the robot's end effector in the workspace are acquired through the robot operating system; the environmental data and human motion trajectory are acquired in real time using a motion capture device; S2, establishing a model: establishing a multi-objective, multi-constraint trajectory optimization model based on the data obtained in step S1; S3, Problem transformation and solution: The nonlinear constraints of the trajectory optimization problem are transformed into penalty functions through the trust region-based optimization method, and the non-convex optimization problem is iteratively solved through a series of approximate problems; S4, trajectory division and optimization execution: The original trajectory is divided into coarse trajectory clusters through the incremental trajectory optimization framework, the environmental information is input at a certain frequency, the trajectory is optimized, and the feasible trajectory of the corresponding time period is executed.

2. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 1 is characterized in that: The specific process of establishing the model in step S2 includes: Determine multiple objective functions for trajectory optimization, including path length, energy consumption, obstacle avoidance capability, etc.; set multiple constraints for trajectory optimization, including speed constraints, acceleration constraints, joint angle constraints, etc.; construct a mathematical model of the objective function and constraints, and express it in the form of an optimization problem to form a multi-objective, multi-constraint optimization problem.

3. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 2 is characterized in that: The specific process of problem transformation and solution in step S3 includes: S3.1, Constraint transformation: Use penalty function method to deal with nonlinear constraints and transform the constraints into part of the objective function; S3.2, Optimization problem transformation: Construct trust region subproblems to transform the original non-convex optimization problem into a series of approximate convex optimization problems; S3.

3. Solve the optimal solution: gradually approach the optimal solution by iteratively solving the trust region subproblem.

4. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 3 is characterized in that: The specific process of the constraint condition transformation in step S3.1 includes: Determine the constraints and express them in the form of equations or inequalities; introduce the constraints into the objective function and construct a new objective function, that is, add the penalty function term to the original objective function; select appropriate penalty function parameters (penalty factor) to determine whether the optimization result meets the constraints. If not, continuously increase the penalty factor to ensure that the penalty function can effectively constrain the optimization process.

5. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 3 is characterized in that: The specific process of optimizing the problem transformation in step S3.2 includes: Based on the current solution, a trust region is defined, that is, the local search area of ​​the solution; according to Taylor expansion, an approximate model is constructed in the trust region; the approximate model is used to replace the original problem and the optimization solution is performed in the trust region.

6. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 3 is characterized in that: The specific process of solving the optimal solution in step S3.3 includes: In each iteration step, the approximate model is used to solve the subproblem and obtain a new solution; check whether the new solution meets the requirements of the trust region. If so, update the trust region and the current solution; otherwise, reduce the trust region and continue to solve the subproblem until a solution that meets the requirements is found; in each iteration process, by adjusting the size and position of the trust region, the search range is gradually narrowed, the solution is continuously updated, and the global optimal solution is gradually approached; through multiple iterations, it is ensured that the final solution is optimal in the global scope.

7. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 1 is characterized in that: The specific process of dividing the trajectory and optimizing the execution in step S4 includes: S4.1, Divide coarse trajectory clusters: Initial trajectory generation, divide the original trajectory into coarse trajectory clusters; S4.2, optimize the coarse trajectory cluster: incremental optimization, gradually input the real-time environment information, and perform local optimization on the coarse trajectory cluster; S4.

3. Execute feasible trajectories: Execute the optimized trajectory clusters in sequence according to time periods to ensure the continuity and safety of the robot's motion.

8. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 7 is characterized in that: The specific process of dividing the coarse trajectory clusters in step S4.1 includes: According to the length and complexity of the initial trajectory, it is divided into several segments to form a coarse trajectory cluster; Each rough trajectory contains a series of trajectory points, which represent the spatiotemporal information of the robot's running trajectory, and are represented by the control vector of the robot's configuration space and the time interval between the trajectory points.

9. The incremental trust region-based robot multi-objective trajectory optimization method according to claim 8, characterized in that: The specific process of optimizing the rough trajectory cluster in step S4.2 includes: The current environmental information is obtained through sensors, and the internal parameters of the robot trajectory optimizer are updated according to the robot trajectory executed in the previous optimization cycle, the observed human trajectory and the current environmental state; according to the current human motion state, the human motion trajectory is predicted through the uniform motion model to obtain the predicted trajectory of the human body in the next optimization cycle; the optimizer optimizes the trajectory according to the set goals and constraints, using an improved trajectory optimization method based on trust region to generate an optimized trajectory.

10. An electronic device comprising a memory and a processor, wherein a computer program is stored in the memory, wherein: When the processor executes the computer program, any step in the incremental trust region-based robot multi-objective trajectory optimization method as described in any one of claims 1-9 is implemented.

Images