Multi-degree-of-freedom robot inverse kinematics decoupling method based on tornado optimization algorithm

Abstract

The invention discloses a multi-degree-of-freedom robot inverse kinematics decoupling method based on a tornado optimization algorithm, and belongs to the field of robot control. The invention discloses a multi-degree-of-freedom robot inverse kinematics decoupling method based on a tornado optimization algorithm. The multi-degree-of-freedom robot inverse kinematics decoupling method comprises the following steps that 1, robot body parameters and a target pose Pd are input; 2, establishing a forward kinematics pose homogeneous coordinate transformation matrix equation through an MD-H model; 3, converting the target pose Pd into a target pose matrix Td; 4, designing a self-adaptive weighted fitness function and initializing a self-adaptive weight coefficient; the multi-degree-of-freedom robot inverse kinematics decoupling method based on the tornado optimization algorithm, disclosed by the invention, has the beneficial effects that the robot inverse kinematics decoupling problem is converted into the optimization problem, the method is not restrained by robot configuration, and the universality is high.

Description

Technical Field

[0001] This invention belongs to the field of robot control, specifically relating to a decoupling method for inverse kinematics of multi-degree-of-freedom robots based on a tornado optimization algorithm. Background Technology

[0002] With the rapid development of robotics technology, multi-degree-of-freedom (6 degrees of freedom and above) robots are playing an increasingly important role in intelligent manufacturing. Multi-degree-of-freedom robots have been widely applied in various industrial manufacturing fields. Multi-degree-of-freedom robots need to perform corresponding movements and operations based on a task space (Cartesian space), which inevitably involves robot inverse kinematics. Robot inverse kinematics decoupling refers to calculating the joint angle values ​​of each joint in its own joint space given the position and orientation of the robot's end effector relative to a reference coordinate system. It is the foundation and key core of robot motion control. The multi-degree-of-freedom robot inverse kinematics decoupling problem is characterized by high dimensionality, multiple variables, strong coupling, and high nonlinearity.

[0003] The invention patent application (publication number: CN118163113A) discloses a "method for solving the inverse kinematics of a robot with redundant degrees of freedom based on the dung beetle optimization algorithm," which improves the efficiency and accuracy of the solution through the dung beetle optimization algorithm. However, this method uses a proximity vector, i.e., a posture vector, for the attitude error in the fitness function. In some cases, the attitude may have singularity problems, such as Euler angle gimbal lock-up, leading to unstable solutions and poor robustness. If the attitude vector is used to construct the fitness function, since the complete attitude vector is 9-dimensional, plus the 3-dimensional position, the pose optimization term alone will have as many as 12 dimensions, significantly increasing the computational load. The joint angle change optimization term in the fitness function only considers Euclidean distance and does not consider physical characteristics such as link length and link mass, making it difficult to achieve a comprehensive optimal solution that is compatible with high efficiency and high performance. In addition, the weight coefficients in the adaptive function are fixed values, and the weight ratios of each error term cannot be dynamically adjusted according to task requirements, resulting in weak adaptive capability.

[0004] The Tornado Optimizer with Coriolis force (TOC algorithm) is a novel metaheuristic algorithm inspired by the formation and movement of tornadoes in nature, primarily involving the evolution of storms and thunderstorms into tornadoes using the Coriolis force, as well as random new storms. Currently, there are no technical insights or teachings for applying the Tornado Optimizer algorithm to specific industrial control fields. In other words, the Tornado Optimizer algorithm cannot be directly applied to the field of robot control, nor can it achieve the following technical problems that this invention aims to solve: 1. The inverse kinematics decoupling problem of multi-degree-of-freedom robots is transformed into a multi-objective constrained optimization problem, which is not constrained by the robot configuration and has strong versatility.

[0005] 2. By designing an adaptive weighted fitness function, the various error optimization terms can be dynamically adjusted according to different task requirements. The attitude error term uses quaternion error terms, which avoids singularity, has higher robustness, and can describe the differences in attitude in three-dimensional space more smoothly and accurately. It is more suitable for solving complex and high-precision attitude problems, and reduces the optimization solution dimension, which greatly improves the computational efficiency. By integrating multiple physical characteristic error optimization terms such as position, attitude, distance, link length, and link mass, a comprehensive optimal solution with high efficiency, high precision, and high performance is achieved. Summary of the Invention

[0006] This invention addresses the shortcomings of existing technologies by providing a multi-degree-of-freedom robot inverse kinematics decoupling method based on the tornado optimization algorithm. This method is not constrained by the configuration of the multi-degree-of-freedom robot, is applicable to multi-degree-of-freedom robots of any configuration, exhibits strong versatility, no singularity, high robustness, strong adaptability, and high flexibility. It can dynamically adjust weights according to different task requirements, and simultaneously achieves fast convergence and high computational efficiency. This method enables high-efficiency, high-precision, and high-performance comprehensive optimal solutions, meeting the requirements of high real-time control.

[0007] This invention employs the following technical solution: a multi-degree-of-freedom robot inverse kinematics decoupling method based on the tornado optimization algorithm, comprising the following steps: Step 1: Input robot body parameters and target pose P d ; Step 2: Establish the homogeneous coordinate transformation matrix equation for the positive motion pose using the MD-H model; Step 3: Set the target pose P d Converted into target pose matrix T d ; Step 4: Design the adaptive weighted fitness function and initialize the adaptive weight coefficients; Step 5: Update the fitness value based on the adaptive weighted fitness function; Step 6: Optimize and solve the inverse kinematics decoupling problem of a multi-degree-of-freedom robot based on the tornado optimization algorithm; Step 7: Determine the robot's global optimal position solution.

[0008] As a preferred technical solution to the above technical solutions, in step 1, the robot body parameters include the robot's degrees of freedom n and the joint link length L. i Connecting rod mass M i Initial joint angle θ 0i and joint angle limit θ min_i θ max_i Target pose P d Specifically, the target pose Pd (x d , y d , z d , a d , b d , c d ); where x d y d z d For the position component, a d b d c d Let i be the attitude components, i = 1, 2, 3…n.

[0009] As the preferred technical solution above, step 2 is specifically implemented as follows: Step 2.1: Based on the MD-H model, the homogeneous coordinate transformation matrix of the {i}th joint coordinate system relative to the {i-1}th joint coordinate system is: (1) Among them, i=1,2,3...n; θ i Joint angle; L i-1 D is the length of the link at joint i-1; i α is the link offset distance; i-1 This is the deflection angle of the connecting rod; Step 2.2: By multiplying the homogeneous coordinate transformation matrices of each joint, the homogeneous coordinate transformation matrix of the end effector coordinate system {i} of the multi-degree-of-freedom robot relative to the base coordinate system {0} is obtained. The equation of the homogeneous coordinate transformation matrix for the positive motion pose is established as follows: (2).

[0010] As the preferred technical solution above, step 3 is specifically implemented as follows: Set the target pose P d Converted into target pose matrix T d Target pose matrix T d as follows: (3) in, , , , , , , , , , , , .

[0011] As the preferred technical solution above, step 4 is specifically implemented as follows: Step 4.1: Extract the target attitude matrix T according to equation (3). rd and target position matrix P td as follows: (4) (5) Since attitude quaternions are not singular, the target attitude matrix T rd Convert to target pose quaternion Q d (q 1d ,q 2d ,q 3d ,q 4d The details are as follows: (6) The adaptive weighted fitness function is designed as follows: (7) Where t is the iteration number, k q k represents the adaptive weighting coefficients for the attitude quaternion. p k represents the position-adaptive weighting coefficient. s For the adaptive weighting coefficients of the comprehensive optimization term; e q For attitude quaternion error terms; e p e is the position error term; s To comprehensively optimize the error term, the adaptive weighting coefficients are defined as follows: (8) Where, k 0q The basic weights for the attitude quaternion; k 0p For positional basis weights; k 0s The basic weights of the comprehensive optimization term; tanh(•) is the hyperbolic tangent function; λ is the adjustment factor; e ref As an error reference standard; e ref (t) is defined using the global mean as follows: (9) Attitude quaternion error term e q The definition is as follows: (10) Among them, Q c (t) represents the current actual attitude quaternion; Position error term e p The definition is as follows: (11) Among them, Pc (t) represents the current actual position matrix; Comprehensive optimization error term e s The definition is as follows: (12) Among them, w i The overall optimization influence weight of joint i, when L i ≠ 0, w i = L i •M i When L i When = 0, w i =M i ; ; Step 4.2: Set the basic weights k of the attitude quaternion 0q The positional base weight k is 0.4. 0p The basic weight k of the comprehensive optimization term is 0.4. 0s The value is 0.2, and the adjustment factor λ is 1.7.

[0012] As the preferred technical solution above, step 5 is specifically implemented as follows: Step 5.1: Input the current joint angle θ ti The current iteration number t; Step 5.2: Calculate the current pose matrix T using equation (2). c Then, from T using equations (4) and (5) c Extract the current attitude matrix T rc and position matrix P c ; Step 5.3: Use equation (6) to convert the current attitude matrix T rc Convert to attitude quaternion Q c ; Step 5.4: Calculate the attitude quaternion error term e using equations (10), (11), and (12) respectively. q Position error term e p And the comprehensive optimization error term e s ; Step 5.5: Calculate the global reference error e using equation (9). ref ; Step 5.6: Calculate the adaptive weighting coefficient k using equation (8). q k p and k s ; Step 5.7: Normalize the calculated adaptive weight coefficients, as follows: (13) Step 5.8: Substitute the normalized adaptive weight coefficients into equation (7) to update the fitness value.

[0013] As the preferred technical solution above, step 6 is specifically implemented as follows: Step 6.1: Initialize TOC parameters and population; Step 6.2: Calculate the initial fitness values ​​and sort the categories; Step 6.3: Update TOC adaptive parameters and storm speed; Step 6.4: Demonstrate the evolution of the storm into a tornado; Step 6.5: Proceed with the storm's evolution into a thunderstorm; Step 6.6: Proceed with the evolution of thunderstorms into tornadoes; Step 6.7: Randomly generate new storms; Step 6.8: Update the robot's global optimal position solution.

[0014] As the preferred technical solution above, step 6.1 is specifically implemented as follows: Given the TOC parameters: iteration counter t, maximum number of iterations T, solution accuracy convergence threshold ε, search space dimension d, total population size U, and number of storms u. w Thunderstorm quantity u t Number of tornadoes u to , where U=u w +u t +u to ; The initial population is generated randomly, as follows: (14) Where rand is any randomly generated number in the range [0,1], and y i,j It is the initial value of the i-th individual in the j-th dimension, u j and l j These correspond to the upper and lower limits of the search space, respectively.

[0015] As the preferred technical solution above, step 6.2 is specifically implemented as follows: The initial fitness value is calculated using a randomly generated initial population and equation (7), and is represented in vector form as follows: (15) Among them, fit i This indicates that the fitness function value is obtained based on the i-th individual, fit i The specific format is as follows: (16) Among them, yi,d This represents the d-th dimension fitness function value of the i-th individual; The initial fitness values ​​were sorted and divided into three categories: tornadoes, thunderstorms, and storms.

[0016] As the preferred technical solution above, step 6.3 is specifically implemented as follows: Enter the main loop structure for optimization iteration and update the TOC adaptive parameters, as follows: (17) Where ν is the exponential parameter that generates small numbers, e is the natural constant, μ is the fuzzy adaptive storm kinetic energy, and a y The exponential parameter is a0; a0 is a constant value; R l R is the radius of curvature of the storm's trajectory in the Northern Hemisphere. r The radius of curvature of the storm path in the Southern Hemisphere; Storm speed is updated based on Coriolis force, as follows: (18) Among them, i=1,2,3…u w ; This represents the new velocity vector of the i-th storm; The current velocity vector of the i-th storm is represented by ; c represents random numbers created within different ranges; f represents the Coriolis parameter; CF l The Coriolis force in the Northern Hemisphere; CF r η represents the Coriolis force in the Southern Hemisphere; η represents the contraction factor of storm convergence behavior, specifically defined as follows: (19) Where χ represents the acceleration rate of the storm, which is 4.10.

[0017] As the preferred technical solution above, step 6.4 is specifically implemented as follows: The evolution of a storm into a tornado is simulated using a mathematical model, as follows: (20) in, This represents the next position vector of the i-th storm at iteration t+1; This represents the current position vector of the i-th storm at iteration t; This represents the current position vector of the i-th tornado at iteration t; This indicates the difference between a storm evolving into a tornado and a storm forming randomly; rand w And α represent random values, specifically defined as follows: (twenty one) Substitute all storm locations into equation (2) to calculate fitness values, compare them, and update the locations.

[0018] As the preferred technical solution above, step 6.5 is specifically implemented as follows: The process of storms evolving into thunderstorms is simulated as follows: (twenty two) in, and These represent the next and current position vectors of the storm that evolved into a thunderstorm at iterations t+1 and t, respectively. This represents the current position vector of the i-th thunderstorm at the t-th iteration. Indicates the number of storms that evolved or were assigned to a specified thunderstorm or tornado; Substitute all storms and thunderstorms into equation (2) to calculate the fitness value. If the solution of a storm is better than the solution of its adjacent thunderstorm, then swap the positions of the thunderstorm and the storm.

[0019] As the preferred technical solution above, step 6.6 is specifically implemented as follows: The new locations during the evolution of thunderstorms into tornadoes are simulated according to the following definitions: (twenty three) in, and Let represent the next and current position vectors of the thunderstorms that evolved into tornadoes at iterations t+1 and t, respectively. Let δ represent the position vector of the tornado at a random index δ. Represents a random index vector The location vector of the thunderstorm is calculated; all thunderstorm locations are substituted into equation (2) to calculate the fitness value and compare them, and the location is updated.

[0020] As the preferred technical solution above, step 6.7 is specifically implemented as follows: The random formation of storms enhances exploration capabilities and prevents the TOC from getting stuck in local optima and premature convergence. The specific process is as follows: When a storm and a tornado are very close, the storm can be randomly formed in the following way: (twenty four) Where l and u represent the bottom and top limits of the search region, respectively, and δ2 represents the sign change, as detailed below: (25) Where sign(•) represents the sign function; When storms and thunderstorms are very close together, storms are randomly formed in the following way: (26) The search intensity near the tornado is adjusted by ν.

[0021] As the preferred technical solution above, step 6.8 is specifically implemented as follows: The position of the individual with the smallest fitness value in the above tornado evolution process is selected as the global optimal position solution for the current iteration number t.

[0022] As the preferred technical solution above, step 7 is specifically implemented as follows: determine whether the current iteration number t is less than the maximum iteration number T and whether the solution accuracy has reached the solution accuracy convergence threshold ε. If t≤T and the solution accuracy convergence threshold ε has not been reached, then t=t+1, return to step 4 to update the adaptive weight coefficient, calculate the new adaptive weighted fitness function, and continue to perform the next round of optimization. If t>T or the solution accuracy convergence threshold ε has been reached, then output the robot's global optimal position solution.

[0023] The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm disclosed in this invention has the following advantages: 1. By transforming the robot inverse kinematics decoupling problem into an optimization problem, it is not constrained by the robot configuration and has strong versatility.

[0024] 2. By designing an adaptive weighted fitness function, the error optimization terms can be dynamically adjusted according to different task requirements.

[0025] 3. The attitude calculation uses quaternion error terms, which avoids singularity, has higher robustness, can describe the differences in attitude in three-dimensional space more smoothly and accurately, is more suitable for solving complex and high-precision attitude problems, and reduces the optimization solution dimension, which greatly improves computational efficiency.

[0026] 4. By integrating multiple physical characteristic error optimization terms such as position, attitude, distance, link length, and link mass, a comprehensive optimal solution with high efficiency, high precision, and high performance is achieved.

[0027] 5. By using the Tornado optimization algorithm, the optimization solution has strong global search capability and faster convergence speed. It does not depend on the model and training set, does not require a large amount of data annotation, has low computational cost, and meets the requirements of high real-time robot control. Attached Figure Description

[0028] Figure 1 This is a flowchart of the method of the present invention.

[0029] Figure 2This is a flowchart of the adaptive weight adjustment and fitness value update of the present invention.

[0030] Figure 3 This is a comparison chart of the convergence of the fitness function values ​​of the tornado optimization algorithm and the RIME algorithm of this invention. Detailed Implementation

[0031] This invention discloses a method for decoupling inverse kinematics of a multi-degree-of-freedom robot based on a tornado optimization algorithm. The following describes a preferred embodiment (Example 1), with reference to the accompanying drawings. Figures 1 to 3 The specific embodiments of the present invention will be further described below.

[0032] Example 1.

[0033] This embodiment discloses a multi-degree-of-freedom robot inverse kinematics decoupling method based on the tornado optimization algorithm, as shown in the attached figures. Figure 1 The overall concept is: input robot degrees of freedom n, target pose P d Joint link length L i Connecting rod mass M i Initial joint angle θ 0i The process involves: limiting joint angles (where i = 1, 2, 3…n); establishing the homogeneous transformation matrix equation for positive motion poses using the MD-H model (an improved DH model); designing and initializing the adaptive weighted fitness function; initializing the TOC parameters and population; calculating the initial fitness values ​​and sorting them; updating the TOC adaptive parameters and storm speed, then performing storm-to-tornado evolution; performing storm-to-thunderstorm evolution; performing thunderstorm-to-tornado evolution; randomly forming new storms; updating the global optimal location solution; determining whether the maximum number of iterations and the required solution accuracy have been reached; if not, updating the number of iterations, updating the adaptive weights, calculating the new fitness function, and continuing the location optimization; if the stopping condition is met, outputting the optimal location solution and ending the process.

[0034] Based on multiple experiments, the multi-degree-of-freedom robot inverse kinematics decoupling method disclosed in this embodiment, based on the tornado optimization algorithm, is not constrained by robot configuration and has strong versatility. By designing an adaptive weighted fitness function, it can dynamically adjust each error optimization term according to different task requirements. The posture error term uses quaternions, avoiding singularity and exhibiting higher robustness. It can more smoothly and accurately describe the differences in posture in three-dimensional space, making it more suitable for complex and high-precision posture solutions. It also reduces the optimization solution dimension, greatly improving computational efficiency. By integrating multiple physical characteristic error optimization terms such as position, posture, distance, link length, and link mass, it achieves a comprehensive optimal solution with high efficiency, high precision, and high performance. As a novel metaheuristic intelligent optimization algorithm, the tornado optimization algorithm simulates the formation and movement process of natural tornadoes. It has strong global search capabilities and faster convergence speed, does not depend on models or training sets, requires no large amount of data annotation, has low computational cost, and meets the requirements of high real-time robot control.

[0035] Preferably, the inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm includes the following steps: Step 1: Input robot body parameters and target pose P d ; Step 2: Establish the homogeneous coordinate transformation matrix equation for the positive motion pose using the MD-H model; Step 3: Set the target pose P d Converted into target pose matrix T d ; Step 4: Design the adaptive weighted fitness function and initialize the adaptive weight coefficients; Step 5: Update the fitness value based on the adaptive weighted fitness function; Step 6: Optimize and solve the inverse kinematics decoupling problem of a multi-degree-of-freedom robot based on the tornado optimization algorithm; Step 7: Determine the robot's global optimal position solution.

[0036] In step 1, the robot body parameters include the robot's degrees of freedom n and the joint link length L. i Connecting rod mass M i Initial joint angle θ 0i and joint angle limit θ min_i θ max_i Target pose P d Specifically, the target pose P d (x d , y d , z d ,a d , b d , c d ); where x dy d z d For the position component, a d b d c d Let i be the attitude components, i = 1, 2, 3…n.

[0037] Step 2 is specifically implemented as follows: Step 2.1: Based on the MD-H model, the homogeneous coordinate transformation matrix of the {i}th joint coordinate system relative to the {i-1}th joint coordinate system is: (1) Among them, i=1,2,3...n; θ i Joint angle; L i-1 D is the length of the link at joint i-1; i α is the link offset distance; i-1 This is the deflection angle of the connecting rod; Step 2.2: By multiplying the homogeneous coordinate transformation matrices of each joint, the homogeneous coordinate transformation matrix of the end effector coordinate system {i} of the multi-degree-of-freedom robot relative to the base coordinate system {0} is obtained. The equation of the homogeneous coordinate transformation matrix for the positive motion pose is established as follows: (2).

[0038] Step 3 is specifically implemented as follows: Set the target pose P d Converted into target pose matrix T d Target pose matrix T d as follows: (3) in, , , , , , , , , , , , .

[0039] Specifically, regarding step 4, "designing the adaptive weighted fitness function and initializing the adaptive weight coefficients," the details are as follows: The goal of inverse kinematics decoupling for a multi-degree-of-freedom robot is to make the robot's end effector's current actual pose matrix T... c With the target pose matrix T d Equal, i.e., ||T c –T d ||=0, and at the same time, each joint must satisfy the joint angle constraint, i.e., θmin_i ≤θ i ≤θ max_i Therefore, the inverse kinematics decoupling problem of a multi-degree-of-freedom robot is transformed into a multi-objective optimization constraint problem. This allows for dynamic adjustment of each error optimization term according to different task requirements, achieving a comprehensive optimal solution with no singularity, high robustness, high efficiency, high precision, and high performance. Based on this, since the pose matrix contains two matrices with different dimensions—attitude and position—it is inconvenient to directly substitute them into the fitness function for optimization calculation. Therefore, it is necessary to separate the pose matrix and the position matrix, and the fitness function is designed using the following method.

[0040] Step 4 is specifically implemented as follows: Step 4.1: Extract the target attitude matrix T according to equation (3). rd and target position matrix P td as follows: (4) (5) Since attitude quaternions are not singular, the target attitude matrix T rd Convert to target pose quaternion Q d (q 1d ,q 2d ,q 3d ,q 4d The details are as follows: (6) The adaptive weighted fitness function is designed as follows: (7) Where t is the iteration number, k q k represents the adaptive weighting coefficients for the attitude quaternion. p k represents the position-adaptive weighting coefficient. s For the adaptive weighting coefficients of the comprehensive optimization term; e q For attitude quaternion error terms; e p e is the position error term; s To comprehensively optimize the error term, the adaptive weighting coefficients are defined as follows: (8) Where, k 0q The basic weights for the attitude quaternion; k 0p For positional basis weights; k 0s The basic weights of the comprehensive optimization term; tanh(•) is the hyperbolic tangent function; λ is the adjustment factor; e ref As an error reference standard; e ref (t) is defined using the global mean as follows: (9) Attitude quaternion error term e q The definition is as follows: (10) Among them, Q c (t) represents the current actual attitude quaternion; Position error term e p The definition is as follows: (11) Among them, P c (t) represents the current actual position matrix; Comprehensive optimization error term e s The definition is as follows: (12) Among them, w i The overall optimization influence weight of joint i, when L i ≠ 0, w i = L i •M i When L i When = 0, w i =M i ; ; Step 4.2: Set the basic weights k of the attitude quaternion 0q The positional base weight k is 0.4. 0p The basic weight k of the comprehensive optimization term is 0.4. 0s The value is 0.2, and the adjustment factor λ is 1.7.

[0041] Specifically, regarding "Equation (7)" in step 4.1, the following is a detailed explanation: The adaptive weighted fitness function designed in Equation (7) can dynamically adjust each error optimization term according to different task requirements. When a certain error is significantly larger, the corresponding weight coefficient will increase, automatically strengthening the optimization intensity of that term. When each error term converges at a similar rate, the weight coefficient remains at the basic weight to promote optimization in a balanced manner. When a certain error is close to convergence, the corresponding weight coefficient maintains the basic weight to avoid over-converging terms. The attitude uses quaternion error terms, which avoids singularity, has higher robustness, can describe the three-dimensional spatial attitude differences more smoothly and accurately, is more suitable for complex high-precision attitude solutions, and reduces the optimization solution dimension, greatly improving computational efficiency. By integrating multiple physical characteristic error optimization terms such as position, attitude, distance, link length, and link mass, a comprehensive optimal solution with high efficiency, high precision, and high performance is achieved.

[0042] Step 5 is specifically implemented as follows: Step 5.1: Input the current joint angle θti The current iteration number t; Step 5.2: Calculate the current pose matrix T using equation (2). c Then, from T using equations (4) and (5) c Extract the current attitude matrix T rc and position matrix P c ; Step 5.3: Use equation (6) to convert the current attitude matrix T rc Convert to attitude quaternion Q c ; Step 5.4: Calculate the attitude quaternion error term e using equations (10), (11), and (12) respectively. q Position error term e p And the comprehensive optimization error term e s ; Step 5.5: Calculate the global reference error e using equation (9). ref ; Step 5.6: Calculate the adaptive weighting coefficient k using equation (8). q k p and k s ; Step 5.7: Normalize the calculated adaptive weight coefficients, as follows: (13) Step 5.8: Substitute the normalized adaptive weight coefficients into equation (7) to update the fitness value.

[0043] Step 6 is specifically implemented as follows: Step 6.1: Initialize TOC parameters and population; Step 6.2: Calculate the initial fitness values ​​and sort the categories; Step 6.3: Update TOC adaptive parameters and storm speed; Step 6.4: Demonstrate the evolution of the storm into a tornado; Step 6.5: Proceed with the storm's evolution into a thunderstorm; Step 6.6: Proceed with the evolution of thunderstorms into tornadoes; Step 6.7: Randomly generate new storms; Step 6.8: Update the robot's global optimal position solution.

[0044] Step 6.1 is specifically implemented as follows: Given the TOC parameters: iteration counter t (initially 1), maximum number of iterations T, solution accuracy convergence threshold ε, search space dimension d (i.e., robot degrees of freedom n), total population size U, and number of storms u. w Thunderstorm quantity ut Number of tornadoes u to , where U=u w +u t +u to ; The initial population is generated randomly, as follows: (14) Where rand is any randomly generated number in the range [0,1], and y i,j It is the initial value of the i-th individual in the j-th dimension, u j and l j These correspond to the upper and lower bounds of the search space (i.e., the upper and lower bounds θ for each joint). max_i and θ min_i ).

[0045] Step 6.2 is specifically implemented as follows: The initial fitness value is calculated using a randomly generated initial population and equation (7), and is represented in vector form as follows: (15) Among them, fit i This indicates that the fitness function value is obtained based on the i-th individual, fit i The specific format is as follows: (16) Among them, y i,d This represents the d-th dimension fitness function value of the i-th individual; The initial fitness values ​​are sorted and divided into three categories: tornado (optimal solution), thunderstorm (suboptimal solution), and storm (normal solution).

[0046] Step 6.3 is specifically implemented as follows: Enter the main loop structure for optimization iteration and update the TOC adaptive parameters, as follows: (17) Where ν is the exponential parameter that generates small numbers, e is the natural constant, μ is the fuzzy adaptive storm kinetic energy, and a y For exponent parameters; a0 is a constant value (2.0); R l R is the radius of curvature of the storm's trajectory in the Northern Hemisphere. r The radius of curvature of the storm path in the Southern Hemisphere; Storm speed is updated based on Coriolis force, as follows: (18) Among them, i=1,2,3…u w ; This represents the new velocity vector of the i-th storm; The current velocity vector of the i-th storm is represented by ; c represents random numbers created within different ranges; f represents the Coriolis parameter; CF l The Coriolis force in the Northern Hemisphere; CF r η represents the Coriolis force in the Southern Hemisphere; η represents the contraction factor of storm convergence behavior, specifically defined as follows: (19) Where χ represents the acceleration rate of the storm, which is 4.10.

[0047] Step 6.4 is specifically implemented as follows: The evolution of a storm into a tornado is simulated using a mathematical model, as follows: (20) in, This represents the next position vector of the i-th storm at iteration t+1; This represents the current position vector of the i-th storm at iteration t; This represents the current position vector of the i-th tornado at iteration t; This indicates the difference between a storm evolving into a tornado and a storm forming randomly; rand w And α represent random values, specifically defined as follows: (twenty one) Substitute all storm locations into equation (2) to calculate fitness values, compare them, and update the locations.

[0048] Step 6.5 is specifically implemented as follows: The process of storms evolving into thunderstorms is simulated as follows: (twenty two) in, and These represent the next and current position vectors of the storm that evolved into a thunderstorm at iterations t+1 and t, respectively. This represents the current position vector of the i-th thunderstorm at the t-th iteration. Indicates the number of storms that evolved or were assigned to a specified thunderstorm or tornado; Substitute all storms and thunderstorms into equation (2) to calculate the fitness value. If the solution of a storm is better than the solution of its adjacent thunderstorm, then swap the positions of the thunderstorm and the storm.

[0049] Step 6.6 is specifically implemented as follows: The new locations during the evolution of thunderstorms into tornadoes are simulated according to the following definitions: (twenty three) in, and Let represent the next and current position vectors of the thunderstorms that evolved into tornadoes at iterations t+1 and t, respectively. Let δ represent the position vector of the tornado at a random index δ. Represents a random index vector The location vector of the thunderstorm is calculated; all thunderstorm locations are substituted into equation (2) to calculate the fitness value and compare them, and the location is updated.

[0050] Step 6.7 is specifically implemented as follows: The random formation of storms enhances exploration capabilities and prevents the TOC from getting stuck in local optima and premature convergence. The specific process is as follows: When a storm and a tornado are very close, the storm can be randomly formed in the following way: (twenty four) Where l and u represent the bottom and top limits of the search region, respectively, and δ2 represents the sign change, as detailed below: (25) Where sign(•) represents the sign function; When storms and thunderstorms are very close together, storms are randomly formed in the following way: (26) The search intensity near the tornado is adjusted by ν.

[0051] Step 6.8 is specifically implemented as follows: The position of the individual with the smallest fitness value in the above tornado evolution process is selected as the global optimal position solution for the current iteration number t.

[0052] Specifically, step 7 is implemented as follows: determine whether the current iteration number t is less than the maximum iteration number T and whether the solution accuracy has reached the solution accuracy convergence threshold ε. If t≤T and the solution accuracy convergence threshold ε has not been reached, then t=t+1, return to step 4 to update the adaptive weight coefficients, calculate the new adaptive weighted fitness function, and continue to perform the next round of optimization. If t>T or the solution accuracy convergence threshold ε has been reached, then output the robot's global optimal position solution.

[0053] It is worth mentioning that the multi-degree-of-freedom robot inverse kinematics decoupling method based on the tornado optimization algorithm disclosed in this embodiment is not constrained by robot configuration and has strong versatility. It can adaptively adjust the weight coefficients and dynamically adjust each error optimization term according to different task requirements. The posture uses quaternion error terms to avoid singularity, which has higher robustness and can describe the differences in three-dimensional spatial posture more smoothly and accurately. It is more suitable for solving complex and high-precision postures and reduces the optimization solution dimension, which greatly improves the computational efficiency. By integrating multiple physical characteristic error optimization terms such as position, posture, distance, link length, and link mass, it achieves a comprehensive optimal solution with high efficiency, high precision, and high performance. It has strong global search capabilities and faster convergence speed, does not depend on models and training sets, does not require a large amount of data annotation, has low computational load, and meets the requirements of high real-time robot control. Experimental verification is now being carried out.

[0054] In the experiment, a typical six-DOF robot was used as the verification object. Detailed MD-H model parameters are shown in Table 1. Given the desired target pose of the robot's end effector, the robot's real-time control system performs inverse kinematics decoupling to solve for the corresponding joint angles. In the experiment, the maximum number of algorithm iterations was set to 100, the search space dimension to 6, the total population size to 300, and the solution accuracy convergence threshold ε to 1E-15.

[0055] Table 1

[0056]

[0057] In the experiment, 10 sets of desired target poses for the robot's end effector were given, as shown in Table 2-1. Among them, the position component x... d y d z d The unit is m, and the attitude component is a. d b d c d The unit is rad. Table 2-2 shows the joint angles (rad) obtained by inverse kinematic decoupling using the tornado optimization algorithm of this invention. Table 2-3 shows the errors between the target pose and the inverse kinematic decoupling pose. As can be seen from the table, the position error is between 1E-18 and 1E-16, and the posture error is between 1E-17 and 1E-16. This fully demonstrates that the tornado optimization algorithm of this invention can achieve high-precision robot inverse kinematic decoupling.

[0058] Table 2-1

[0059] Table 2-2

[0060] Table 2-3

[0061] like Figure 3 The image shows a comparison of the convergence of the fitness function values ​​between the Tornado optimization algorithm and the more recent RIME (Frost Ice Optimization) algorithm, both used under the same settings for robot inverse kinematics decoupling. Figure 3 As can be seen, the Tornado optimization algorithm achieves stable convergence of its fitness function value on the 4th iteration and remains in a convergent state, while the RIME algorithm only begins to converge after 25 iterations. Furthermore, the fitness function value of the Tornado optimization algorithm at convergence is significantly smaller than that of the RIME algorithm. Clearly, the Tornado optimization algorithm has a faster convergence speed and higher convergence accuracy.

[0062] It is worth mentioning that the technical features such as the RIME algorithm involved in this patent application should be regarded as prior art. The specific structure, working principle and possible control methods and spatial arrangement methods of these technical features can be adopted by conventional choices in the field, and should not be regarded as the inventive point of this patent. This patent will not be further elaborated in detail.

[0063] For those skilled in the art, modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this invention should be included within the protection scope of this invention.

Claims

1. A method for decoupling inverse kinematics of a multi-degree-of-freedom robot based on a tornado optimization algorithm, characterized in that, Includes the following steps: Step 1: Input robot body parameters and target pose P d ; Step 2: Establish the homogeneous coordinate transformation matrix equation for the positive motion pose using the MD-H model; Step 3: Set the target pose P d Converted into target pose matrix T d ; Step 4: Design the adaptive weighted fitness function and initialize the adaptive weight coefficients; Step 5: Update the fitness value based on the adaptive weighted fitness function; Step 6: Optimize and solve the inverse kinematics decoupling problem of a multi-degree-of-freedom robot based on the tornado optimization algorithm; Step 7: Determine the robot's global optimal position solution.

2. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 1, characterized in that, In step 1, the robot body parameters include the robot's degrees of freedom n and the joint link lengths L. i Connecting rod mass M i Initial joint angle θ 0i and joint angle limit θ min_i θ max_i Target pose P d Specifically, the target pose P d (x d , y d , z d , a d ,b d , c d ); where x d y d z d For the position component, a d b d c d Let i be the attitude components, i = 1, 2, 3…n.

3. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 1, characterized in that, Step 2 is implemented in the following steps: Step 2.1: Based on the MD-H model, the homogeneous coordinate transformation matrix of the {i}th joint coordinate system relative to the {i-1}th joint coordinate system is: （1） Among them, i=1,2,3...n; θ i Joint angle; L i-1 D is the length of the link at joint i-1; i α is the link offset distance; i-1 This is the deflection angle of the connecting rod; Step 2.2: By multiplying the homogeneous coordinate transformation matrices of each joint, the homogeneous coordinate transformation matrix of the end effector coordinate system {i} of the multi-degree-of-freedom robot relative to the base coordinate system {0} is obtained. The equation of the homogeneous coordinate transformation matrix for the positive motion pose is established as follows: （2）。 4. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 3, characterized in that, Step 3 is implemented in the following steps: Set the target pose P d Converted into target pose matrix T d Target pose matrix T d as follows: （3） in, , , , , , , , , , , , .

5. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 4, characterized in that, Step 4 is implemented in the following steps: Step 4.1: Extract the target attitude matrix T according to equation (3). rd and target position matrix P td as follows: （4） （5） Since attitude quaternions are not singular, the target attitude matrix T rd Converted into target pose quaternion Q d (q 1d ,q 2d ,q 3d ,q 4d The details are as follows: （6） The adaptive weighted fitness function is designed as follows: （7） Where t is the iteration number, k q k represents the adaptive weighting coefficients for the attitude quaternion. p k represents the position-adaptive weighting coefficient. s For the adaptive weighting coefficients of the comprehensive optimization term; e q For attitude quaternion error terms; e p For the position error term; e s To comprehensively optimize the error term, the adaptive weighting coefficients are defined as follows: （8） Where, k 0q The basic weights for the attitude quaternion; k 0p For positional basis weights; k 0s The basic weights of the comprehensive optimization term; tanh(•) is the hyperbolic tangent function; λ is the adjustment factor; e ref As an error reference standard; e ref (t) is defined using the global mean as follows: （9） Attitude quaternion error term e q The definition is as follows: （10） Among them, Q c (t) represents the current actual attitude quaternion; Position error term e p The definition is as follows: （11） Among them, P c (t) represents the current actual position matrix; Comprehensive optimization error term e s The definition is as follows: （12） Among them, w i The overall optimization influence weight of joint i, when L i ≠ 0, w i = L i •M i When L i When = 0, w i = M i ; ; Step 4.2: Set the basic weights k of the attitude quaternion 0q The positional base weight k is 0.

4. 0p The basic weight k of the comprehensive optimization term is 0.

4. 0s The value is 0.2, and the adjustment factor λ is 1.

7.

6. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 5, characterized in that, Step 5 is implemented in the following steps: Step 5.1: Input the current joint angle θ ti The current iteration number t; Step 5.2: Calculate the current pose matrix T using equation (2). c Then, from T using equations (4) and (5) c Extract the current attitude matrix T rc and position matrix P c ; Step 5.3: Use equation (6) to convert the current attitude matrix T rc Convert to attitude quaternion Q c ; Step 5.4: Calculate the attitude quaternion error term e using equations (10), (11), and (12) respectively. q Position error term e p and the comprehensive optimization error term e s ; Step 5.5: Calculate the global reference error e using equation (9). ref ; Step 5.6: Calculate the adaptive weighting coefficient k using equation (8). q k p and k s ; Step 5.7: Normalize the calculated adaptive weight coefficients, as follows: （13） Step 5.8: Substitute the normalized adaptive weight coefficients into equation (7) to update the fitness value.

7. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 6, characterized in that, Step 6 is implemented in the following steps: Step 6.1: Initialize TOC parameters and population; Step 6.2: Calculate the initial fitness values ​​and sort the categories; Step 6.3: Update TOC adaptive parameters and storm speed; Step 6.4: Demonstrate the evolution of the storm into a tornado; Step 6.5: Proceed with the storm's evolution into a thunderstorm; Step 6.6: Proceed with the evolution of thunderstorms into tornadoes; Step 6.7: Randomly generate new storms; Step 6.8: Update the robot's global optimal position solution.

8. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 7, characterized in that, Step 6.1 is implemented in the following steps: Given the TOC parameters: iteration counter t, maximum number of iterations T, solution accuracy convergence threshold ε, search space dimension d, total population size U, and number of storms u. w Thunderstorm quantity u t Number of tornadoes u to , where U=u w +u t +u to ; The initial population is generated randomly, as follows: （14） Where rand is any randomly generated number in the range [0,1], and y i,j It is the initial value of the i-th individual in the j-th dimension, u j and l j These correspond to the upper and lower limits of the search space, respectively.

9. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 8, characterized in that, Step 6.2 is implemented in the following steps: The initial fitness value is calculated using a randomly generated initial population and equation (7), and is represented in vector form as follows: （15） Among them, fit i This indicates that the fitness function value is obtained based on the i-th individual, fit i The specific format is as follows: （16） Among them, y i,d This represents the d-th dimension fitness function value of the i-th individual; The initial fitness values ​​were sorted and divided into three categories: tornadoes, thunderstorms, and storms.

10. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 9, characterized in that, Step 6.3 is implemented in the following steps: Enter the optimization iteration main loop structure and update the TOC adaptive parameters, as follows: （17） Where ν is the exponential parameter that generates small numbers, e is the natural constant, μ is the fuzzy adaptive storm kinetic energy, and a y The exponential parameter is a0; a0 is a constant value; R l R is the radius of curvature of the storm's trajectory in the Northern Hemisphere. r The radius of curvature of the storm path in the Southern Hemisphere; Storm speed is updated based on Coriolis force, as follows: （18） Among them, i=1,2,3…u w ; This represents the new velocity vector of the i-th storm; Represents the current velocity vector of the i-th storm; c represents random numbers created within different ranges; f represents the Coriolis parameter; CF l The Coriolis force in the Northern Hemisphere; CF r η represents the Coriolis force in the Southern Hemisphere; η represents the contraction factor of storm convergence behavior, specifically defined as follows: （19） Where χ represents the acceleration rate of the storm, which is 4.

10.

11. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 10, characterized in that, Step 6.4 is implemented in the following steps: The evolution of a storm into a tornado is simulated using a mathematical model, as follows: （20） in, This represents the next position vector of the i-th storm at iteration t+1; This represents the current position vector of the i-th storm at iteration t; This represents the current position vector of the i-th tornado at iteration t; This indicates the difference between a storm evolving into a tornado and a storm forming randomly; rand w And α represent random values, specifically defined as follows: （21） Substitute all storm locations into equation (2) to calculate fitness values, compare them, and update the locations.

12. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 10, characterized in that, Step 6.5 is implemented in the following steps: The process of storms evolving into thunderstorms is simulated as follows: （22） in, and These represent the next and current position vectors of the storm that evolved into a thunderstorm at iterations t+1 and t, respectively. This represents the current position vector of the i-th thunderstorm at the t-th iteration. Indicates the number of storms that evolved or were assigned to a specified thunderstorm or tornado; Substitute all storms and thunderstorms into equation (2) to calculate the fitness value. If the solution of a storm is better than the solution of its adjacent thunderstorm, then swap the positions of the thunderstorm and the storm.

13. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 10, characterized in that, Step 6.6 is implemented in the following steps: The new locations during the evolution of thunderstorms into tornadoes are simulated according to the following definitions: （23） in, and Let represent the next and current position vectors of the thunderstorms that evolved into tornadoes at iterations t+1 and t, respectively. Let δ represent the position vector of the tornado at a random index δ. Represents a random index vector The location vector of the thunderstorm is calculated; all thunderstorm locations are substituted into equation (2) to calculate the fitness value and compare them, and the location is updated.

14. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 10, characterized in that, Step 6.7 is implemented in the following steps: The random formation of storms enhances exploration capabilities and prevents the TOC from getting stuck in local optima and premature convergence. The specific process is as follows: When a storm and a tornado are very close together, the storm can be randomly formed in the following way: （24） Where l and u represent the bottom and top limits of the search region, respectively, and δ2 represents the sign change, as detailed below: （25） Where sign(•) represents the sign function; When storms and thunderstorms are very close together, storms are randomly formed in the following way: （26） The search intensity near the tornado is adjusted by ν.

15. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 10, characterized in that, Step 6.8 is implemented in the following steps: The position of the individual with the smallest fitness value in the above tornado evolution process is selected as the global optimal position solution for the current iteration number t.

16. The inverse kinematics decoupling method for multi-degree-of-freedom robots based on the tornado optimization algorithm according to claim 1, characterized in that, Step 7 is implemented as follows: Determine whether the current iteration number t is less than the maximum iteration number T and whether the solution accuracy has reached the solution accuracy convergence threshold ε. If t≤T and the solution accuracy convergence threshold ε has not been reached, then t=t+1, return to step 4 to update the adaptive weight coefficients, calculate the new adaptive weighted fitness function, and continue the next round of optimization. If t>T or the solution accuracy convergence threshold ε has been reached, then output the robot's global optimal position solution.

Images