An optimized inverse kinematics solution algorithm for redundant robot snakes

Abstract

This paper discloses a method for solving the inverse kinematics of redundant robots using snake optimization. This method uses the pose errors of the redundant robot's end effectors to construct an optimization objective function, thereby converting the redundant robot's inverse kinematics problem into an equivalent minimum optimization problem. This problem is then optimized and solved using a snake optimization inverse kinematics algorithm. This method can quickly and easily solve the redundant robot's inverse kinematics and accurately obtain the joint positions of the redundant robot's poses.

Description

Technical Field

[0001] The invention relates to a redundant robot snake optimization inverse kinematics solution method, belonging to the field of robot kinematics. Background Art

[0002] Solving the inverse kinematics of redundant robots is the prerequisite and foundation for their motion and trajectory planning. The two main approaches to solving inverse kinematics are closed-form solutions and numerical solutions. Redundant robots with general configurations do not satisfy the Pieper criterion. Their kinematic equations are a system of strongly coupled, nonlinear equations with multiple variables, making it impossible to separate the variables. Furthermore, this system of equations has an infinite number of solutions, necessitating numerical solutions to obtain a satisfactory inverse solution.

[0003] The Snake optimization (SO) algorithm was proposed in 2022. The original snake optimization provides a new solution for solving nonlinear equations due to its superior performance in peaks and multi-dimensions. The inverse kinematics of redundant robots is a multi-variable strongly coupled nonlinear equation system. By applying the SO algorithm to redundant robot kinematics, a solution for redundant robot kinematics is provided. Summary of the Invention

[0004] In response to the problems of difficulty in solving the inverse kinematics of redundant robots and low posture accuracy, the present invention provides an optimized inverse kinematics solution algorithm for redundant robot snakes. The algorithm has the advantages of fast solution speed, high posture accuracy, and easy programming.

[0005] To achieve the above purpose, the attached Figure 1 As shown, the technical solution adopted by the present invention is:

[0006] An optimized inverse kinematics solution algorithm for redundant robot snakes. The initial position vector of the i-th snake individual in the D-dimensional joint space is , which is the candidate inverse solution corresponding to the inverse kinematics problem. The SO algorithm generates intermediate candidate inverse solutions V and the global optimal candidate inverse solution GB in the food search stage, the snake group interaction stage, and the snake egg replacement stage. D is the number of joints related to the posture during robot motion. The algorithm can obtain N feasible inverse solutions in each iteration. The global optimal solution in the optimization process is memorized and saved, and the global optimal solution is used as the final output solution of the algorithm. The algorithm includes the following steps:

[0007] Step 1: Set the algorithm initial value, population size N, maximum number of iterations T, number of joints D, food threshold QT, temperature threshold TT, and establish the objective function , convergence accuracy ;

[0008] The objective function is

[0009] (1)

[0010] In the formula is the robot’s initial pose matrix, is the robot's desired posture matrix; the redundant robot reaches the target position with the desired posture, which must satisfy The minimum value of the objective function is shown in formula (2), and the joint restriction range is shown in formula (3);

[0011] (2)

[0012] (3)

[0013] Step 2: Determine the joint limit range of each joint of the redundant robot according to formula (3) and randomly initialize the joint angle value of each joint. The joint angle random initialization formula is shown in formula (4);

[0014] Step 2-1: Divide the initialized candidate inverse solutions into two groups of male and female, with N / 2 in each group, and obtain N initial candidate inverse solutions. , where i = 1 ~ N, j = 1 ~ D;

[0015] (4)

[0016] in, is the minimum value of the j-th dimension component in the D-dimensional joint space, is the maximum value of the j-th dimension component in the D-dimensional joint space, and r is a random variable uniformly distributed in the range of [0, 1];

[0017] Step 2-2, calculate the fitness value using the objective function according to formula (1) and take the minimum fitness value as the global optimal candidate inverse solution GB. In the process of generating candidate inverse solutions, if the intermediate candidate inverse solution V exceeds the joint space limit, use formula (4) to correct it;

[0018] Step 3: Calculate the food parameter Q and temperature parameter Temp of the environment at the current iteration number t according to formula (5);

[0019] (5)

[0020] Step 4: Determine whether the temperature parameter Temp exceeds the temperature threshold TT and whether the food parameter Q exceeds the food threshold QT;

[0021] When the temperature parameter Temp exceeds the temperature threshold TT and the food parameter Q exceeds the food threshold QT, the snake optimization algorithm food exploration phase 1 is performed, and the inverse solution of each candidate is calculated using formula (6). Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method;

[0022] (6)

[0023] Food foraging ability , is a random candidate inverse solution in the N group;

[0024] Only when the temperature parameter exceeds the temperature threshold TT, the second phase of the snake optimization algorithm food exploration is carried out, and the inverse solution of each candidate is calculated using formula (7). Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method;

[0025] (7)

[0026] When the temperature parameter Temp does not exceed the temperature threshold TT, and the food parameter does not exceed the food threshold QT, the snake optimization snake group interaction stage is carried out; the snake group has a 40% chance of fighting, and for each candidate inverse solution Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution V processing method;

[0027] (8)

[0028] Combat capability , m represents the male group, and f represents the female group;

[0029] There is a 60% chance that the snakes will mate, and during the mating phase, they will also perform inverse analysis on the candidate Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method;

[0030] (9)

[0031] Mating ability , m represents the male group, and f represents the female group;

[0032] After the mating phase is completed, there is a 50% chance of producing snake eggs. The snake egg individuals are initialized and replace the individuals with the worst fitness in the population.

[0033] The processing method of the intermediate candidate inverse solution V is as follows: detect whether the joint angles in the intermediate candidate inverse solution V exceed the joint restriction range, and if so, correct them using formula (4); then, according to the objective function Evaluate the intermediate candidate inverse solution V. If the intermediate candidate inverse solution V is better than the candidate inverse solution , then replace the candidate inverse solution with the intermediate candidate inverse solution V , otherwise keep the candidate inverse solution unchanged; if the intermediate candidate inverse solution V is better than the global optimal candidate inverse solution GB of the previous iteration, the global optimal candidate inverse solution GB is updated with the intermediate candidate inverse solution V, otherwise the optimal candidate inverse solution GB remains unchanged;

[0034] Step 5: Iterate the above steps 3 and 4. When the solution accuracy is better than the convergence accuracy Or when the number of iterations reaches the maximum number T, the optimal candidate inverse solution GB is output.

[0035] It should be pointed out that in the present invention, the position vector of the snake group represents the candidate inverse solution of inverse kinematics. The difference between the position of the snake group and the position of the food corresponds to the quality of the candidate inverse solution. The smaller the fitness value calculated by the objective function, the higher the accuracy of the inverse kinematics solution result.

[0036] Beneficial effect: The present invention provides a redundant robot snake optimization inverse kinematics solution method, which solves the redundant robot inverse kinematics simply and quickly by applying the snake optimization algorithm to the redundant robot inverse kinematics solution, and accurately obtains the joint positions of the redundant robot posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is a flow chart of the optimized inverse kinematics solution method for redundant robot snakes. DETAILED DESCRIPTION

[0038] Example 1 A method for solving inverse kinematics optimization of a redundant robot snake.

[0039] The present invention is a redundant robot snake optimization inverse kinematics solution method, combined with the attached Figure 1 , follow these steps:

[0040] Step 1: Set the population size N to 200, the maximum number of iterations T to 200, the temperature threshold TT to 0.3, the number of joints D to 7, the food threshold QT to 0.35, and the convergence accuracy Better than 10^-10;

[0041] Establishing the objective function for

[0042] (1)

[0043] In the formula is the robot’s initial pose matrix, is the robot's desired posture matrix; the redundant robot reaches the target position with the desired posture, which must satisfy The minimum value of the objective function is shown in formula (2), and the joint restriction range is shown in formula (3);

[0044] (2)

[0045] (3)

[0046] Step 2: Determine the joint limit range of each joint of the redundant robot according to formula (3) and randomly initialize the joint angle value of each joint. The joint angle random initialization formula is as shown in formula (4);

[0047] Step 2-1: Divide the initialized candidate inverse solutions into two groups of male and female, with N / 2 in each group, and obtain N initial candidate inverse solutions. , where i = 1 ~ N, j = 1 ~ D;

[0048] (4)

[0049] in, is the minimum value of the j-th dimension component in the D-dimensional joint space, is the maximum value of the j-th dimension component in the D-dimensional joint space, and r is a random variable uniformly distributed in the range of [0, 1];

[0050] Step 2-2, calculate the fitness value using the objective function according to formula (1) and take the minimum fitness value as the global optimal candidate inverse solution GB. In the process of generating candidate inverse solutions, if the intermediate candidate inverse solution V exceeds the joint space limit, use formula (4) to correct it;

[0051] Step 3: Calculate the food parameter Q and temperature parameter Temp of the environment under the current iteration number t according to formula (5);

[0052] (5)

[0053] Step 4: Determine whether the temperature parameter Temp exceeds the temperature threshold TT and whether the food parameter Q exceeds the food threshold QT;

[0054] When the temperature parameter Temp exceeds the temperature threshold TT and the food parameter Q exceeds the food threshold QT, the snake optimization algorithm food exploration phase 1 is performed, and the inverse solution of each candidate is calculated using formula (6). Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method;

[0055] (6)

[0056] Only when the temperature parameter exceeds the temperature threshold TT, the second phase of the snake optimization algorithm food exploration is carried out, and the inverse solution of each candidate is calculated using formula (7). Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method;

[0057] (7)

[0058] When the temperature parameter Temp does not exceed the temperature threshold TT, and the food parameter does not exceed the food threshold QT, the snake optimization snake group interaction stage is carried out; the snake group has a 40% chance of fighting, and for each candidate inverse solution Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution V processing method;

[0059] (8)

[0060] Combat capability , m represents the male group, and f represents the female group;

[0061] There is a 60% chance that the snakes will mate, and during the mating phase, they will also perform inverse analysis on the candidate Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method;

[0062] (9)

[0063] Mating ability , m represents the male group, and f represents the female group;

[0064] After the mating phase is completed, there is a 50% chance of producing snake eggs. The snake egg individuals are initialized and replace the individuals with the worst fitness in the population.

[0065] The processing method of the intermediate candidate inverse solution V is as follows: detect whether the joint angles in the intermediate candidate inverse solution V exceed the joint restriction range, and if so, correct them using formula (4); then, according to the objective function Evaluate the intermediate candidate inverse solution V. If the intermediate candidate inverse solution V is better than the candidate inverse solution , then replace the candidate inverse solution with the intermediate candidate inverse solution V , otherwise keep the candidate inverse solution unchanged; if the intermediate candidate inverse solution V is better than the global optimal candidate inverse solution GB of the previous iteration, the global optimal candidate inverse solution GB is updated with the intermediate candidate inverse solution V, otherwise the optimal candidate inverse solution GB remains unchanged;

[0066] Step 5: Iterate the above steps 3 and 4. When the solution accuracy is better than the convergence accuracy, Or when the number of iterations reaches the maximum number T, the optimal candidate inverse solution GB is output.

[0067] The experimental results are as follows:

[0068] Given the redundant robot's expected pose matrix:

[0069]

[0070] The global optimal candidate inverse solution GB is [-89.45781, -87.35205, 133.9182, 33.71112, -88.18333, -59.38657, -18.35748], and the convergence accuracy is It is 5.2679×10^-16.

[0071] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A redundant robot snake optimization inverse kinematics solution method, the initial position vector of the i-th snake individual in the D-dimensional joint space , that is, the candidate inverse solution corresponding to the inverse kinematics problem. The SO algorithm generates intermediate candidate inverse solutions V and the global optimal candidate inverse solution GB in the food search stage, the snake group interaction stage, and the snake egg replacement stage. The D is the number of joints related to the posture when the robot moves. The algorithm can obtain N feasible inverse solutions in each iteration, and the global optimal solution in the optimization process will be memorized and saved, and the global optimal solution will be used as the final output solution of the algorithm. It is characterized by The method is divided into the following steps: Step 1: Set the algorithm initial value, population size N, maximum number of iterations T, number of joints D, food threshold QT, temperature threshold TT, and establish the objective function , convergence accuracy ; The objective function is （1） In the formula is the robot’s initial pose matrix, is the robot's desired posture matrix; the redundant robot reaches the target position with the desired posture, which must satisfy , the minimum value of the objective function is shown in formula (2), and the joint restriction range is shown in formula (3); （2） （3） Step 2: Determine the joint limit range of each joint of the redundant robot according to formula (3) and randomly initialize the joint angle value of each joint. The joint angle random initialization formula is shown in formula (4); Step 2-1: Divide the initialized candidate inverse solutions into two groups of male and female, with N / 2 in each group, and obtain N initial candidate inverse solutions. , where i = 1 ~ N, j = 1 ~ D; （4） in, is the minimum value of the j-th dimension component in the D-dimensional joint space, is the maximum value of the j-th dimension component in the D-dimensional joint space, and r is a random variable uniformly distributed in the range of [0, 1]; Step 2-2, calculate the fitness value using the objective function according to formula (1) and take the minimum fitness value as the global optimal candidate inverse solution GB. In the process of generating candidate inverse solutions, if the intermediate candidate inverse solution V exceeds the joint space limit, use formula (4) to correct it; Step 3: Calculate the food parameter Q and temperature parameter Temp of the environment at the current iteration number t according to formula (5); （5） Step 4: Determine whether the temperature parameter Temp exceeds the temperature threshold TT and whether the food parameter Q exceeds the food threshold QT; When the temperature parameter Temp exceeds the temperature threshold TT and the food parameter Q exceeds the food threshold QT, the snake optimization algorithm food exploration phase 1 is performed, and the inverse solution of each candidate is calculated using formula (6). Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method; （6） Food foraging ability ; Only when the temperature parameter exceeds the temperature threshold TT, the second phase of the snake optimization algorithm food exploration is carried out, and the inverse solution of each candidate is calculated using formula (7). Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method; （7） When the temperature parameter Temp does not exceed the temperature threshold TT, and the food parameter does not exceed the food threshold QT, the snake optimization snake group interaction stage is carried out; the snake group has a 40% chance of fighting, and for each candidate inverse solution Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution V processing method; （8） Combat capability , m Representing the male group, f Represents the female group; There is a 60% chance that the snakes will mate, and during the mating phase, they will also perform inverse analysis on the candidate Generate an intermediate candidate inverse solution V, and process the intermediate candidate inverse solution V according to the intermediate candidate inverse solution processing method; （9） Mating ability , m Representing the male group, f Represents the female group; After the mating phase is completed, there is a 50% chance of producing snake eggs. The snake egg individuals are initialized and replace the individuals with the worst fitness in the population. The processing method of the intermediate candidate inverse solution V is as follows: detect whether the joint angles in the intermediate candidate inverse solution V exceed the joint restriction range, and if so, correct them using formula (4); then, according to the objective function Evaluate the intermediate candidate inverse solution V. If the intermediate candidate inverse solution V is better than the candidate inverse solution , then replace the candidate inverse solution with the intermediate candidate inverse solution V , otherwise keep the candidate inverse solution unchanged; if the intermediate candidate inverse solution V is better than the global optimal candidate inverse solution GB of the previous iteration, the global optimal candidate inverse solution GB is updated with the intermediate candidate inverse solution V, otherwise the optimal candidate inverse solution GB remains unchanged; Step 5: Iterate the above steps 3 and 4. When the solution accuracy is better than the convergence accuracy Or when the number of iterations reaches the maximum number T, the optimal candidate inverse solution GB is output.

Images