A kinematics-based redundant manipulator obstacle avoidance trajectory tracking method

Abstract

The application provides a kinematics-based redundant robot obstacle avoidance trajectory tracking method, which comprises the following steps: modeling a target redundant robot through forward kinematics to obtain a forward kinematics equation; modeling an obstacle as a sphere, modeling each link of the target redundant robot and adjacent joints as capsule bodies, and then calculating the minimum distance between the obstacle and each link of the target redundant robot according to the relative position relationship between the obstacle and each capsule body, and establishing an obstacle avoidance penalty function based on the minimum distance; converting the obstacle avoidance trajectory tracking problem of the target redundant robot into a constraint optimization problem, establishing a target function and constraint conditions according to an expected trajectory, the forward kinematics equation and the obstacle avoidance penalty function; searching for an optimal solution of the target function under the constraint conditions by using an improved artificial bee colony algorithm, and taking a group of joint trajectories corresponding to the optimal solution as the obstacle avoidance trajectory of the target redundant robot; and enabling the target redundant robot to realize obstacle avoidance in the process of executing a tracking task.

Description

Technical Field

[0001] This invention relates to the field of industrial robot technology, and in particular to a kinematics-based method for tracking the obstacle avoidance trajectory of a redundant robotic arm. Background Technology

[0002] Redundant robotic arms, due to their biomimetic structure, high flexibility, and repeatability, have been widely used in operations requiring high precision and safety, such as polishing, assembly, and palletizing in smart manufacturing. To safely perform these high-precision tasks in complex environments, accurate trajectory tracking while avoiding obstacles is a crucial issue that redundant robotic arms must address.

[0003] The first step in motion tracking control of redundant robotic arms is solving the inverse kinematics. Existing methods for solving inverse kinematics mainly fall into two categories: closed-form methods and numerical methods. Numerical methods primarily track the trajectory through iteration of the Jacobian matrix; however, the iterative nature of numerical methods makes them much slower than closed-form methods. Closed-form methods, on the other hand, require the redundant robotic arm to satisfy the Piper criterion. Both methods have their limitations in solving the inverse kinematics problem of redundant robotic arms. Therefore, utilizing intelligent optimization algorithms to solve the inverse kinematics problem of redundant robotic arms is a new trend.

[0004] Traditional obstacle avoidance strategies, such as the artificial potential field method and the Bug algorithm, are designed for obstacle avoidance in Cartesian space. For the obstacle avoidance problem of redundant robotic arms, additional calculations are required in joint space, increasing the complexity of the control objective. Some researchers have transformed obstacle avoidance into a quadratic programming problem with inequality constraints, but this method is solved at the velocity level. It requires the initial position of the redundant robotic arm's end effector to be placed on the reference trajectory and iteratively calculates the pseudo-inverse of the Jacobian matrix, thus limiting the workspace of the redundant robotic arm and increasing computational costs. To overcome these drawbacks, it is essential to design a position-level obstacle avoidance and trajectory tracking control method for redundant robotic arms. Summary of the Invention

[0005] This invention provides a kinematics-based method for redundant robotic arm obstacle avoidance trajectory tracking, the purpose of which is to enable the redundant robotic arm to avoid obstacles during the tracking task.

[0006] To achieve the above objectives, this invention provides a kinematics-based redundant robotic arm obstacle avoidance trajectory tracking method, comprising:

[0007] Step 1: Perform forward kinematics modeling on the target redundant robotic arm to obtain the forward kinematics equations;

[0008] Step 2: Model the obstacle as a sphere, and model each target redundant robotic arm link and adjacent joint as a capsule. Based on the relative positional relationship between the obstacle and each capsule, calculate the minimum distance between the obstacle and each link of the target redundant robotic arm, and establish an obstacle avoidance penalty function based on the minimum distance.

[0009] Step 3: Transform the obstacle avoidance trajectory tracking problem of the target redundant robot arm into a constrained optimization problem. Establish the objective function and constraints based on the desired trajectory of the target redundant robot arm, the forward kinematic equation, and the obstacle avoidance penalty function.

[0010] Step 4: Improve the artificial bee colony algorithm by integrating refraction reverse learning, Cauchy mutation strategy and simplex method to obtain the improved artificial bee colony algorithm;

[0011] Step 5: Based on the improved artificial bee colony algorithm, search for the optimal solution of the objective function under the constraints, and use the set of joint trajectories corresponding to the optimal solution as the obstacle avoidance trajectory of the target redundant robotic arm.

[0012] Furthermore, step 1 includes:

[0013] By performing forward kinematic modeling on the target redundant robotic arm, the forward kinematic equations are obtained as follows:

[0014] X ee =f ee (θ)

[0015] Among them, X ee Indicates the pose of the end effector, f ee (·) represents the forward kinematics equation of the end effector, and θ represents a set of joint angles of the target redundant manipulator (the solution of the forward kinematics equation).

[0016] Furthermore, based on the relative positional relationship between the obstacle and each capsule, the minimum distance between the obstacle and each link of the target redundant robotic arm is calculated, and an obstacle avoidance penalty function is established based on the minimum distance, including:

[0017] When the center of the obstacle is O ob On the axis of the capsule The projection on is located The extension line on one side, the center of the obstacle O ob The minimum distance to link i is

[0018] When the center of the obstacle is O ob On the axis of capsule body i The projection on the axis is located on the axis Between, the center of the obstacle O ob The minimum distance to link i is

[0019] When the center of the obstacle is O ob On the axis of capsule body i The projection on is located The extension line on one side, the center of the obstacle O ob The minimum distance to link i is

[0020] Based on minimum distance d i (O,θ), the minimum distance obstacle avoidance penalty function is established as follows:

[0021]

[0022] Where, φ i (·) is the penalty function, α is the penalty coefficient, and d min To minimize the obstacle avoidance distance, d min =r ob +r link +δ d r ob It is the radius of the obstacle, r link It is the radius of the connecting rod, δ d It is a safety redundancy distance.

[0023] Furthermore, the obstacle avoidance trajectory tracking problem of the redundant robotic arm is transformed into a constrained optimization problem, with the objective function being:

[0024]

[0025] Subject to θ i,min ≤θ i (t)≤θ i,max

[0026] φ i (O,θ(t))>d min i = 1, 2, ..., n

[0027] in, Let θ represent the desired end-effector trajectory of the redundant robotic arm, and let θ represent a set of joint angles of the redundant robotic arm. i,min and θ i,max These are the minimum and maximum values ​​of the joint angle.

[0028] Furthermore, the improved artificial bee colony algorithm includes an initialization phase, a hired bee phase, an observation bee phase, a scout bee phase, and a simplex method phase.

[0029] During the initialization phase, the position of the honey source is initialized, and the fitness value of the honey source at the initial position is calculated according to the objective function. There are multiple honey sources, and each honey source corresponds to a set of joint angles of the target redundant robotic arm.

[0030] During the bee-hiring phase, the location of nectar sources is updated by alternating between the refraction-back learning strategy and the Cauchy mutation strategy with different probabilities. The fitness value of the nectar source after the update is calculated and compared with the fitness value of the nectar source before the update. The nectar source with the smaller fitness value is selected.

[0031] During the observation bee phase, the observation bee selects a hired bee to follow, searches for new nectar sources near the hired bee, and calculates the fitness value of each new nectar source to select the nectar source.

[0032] During the scout bee phase, when the selected nectar source reaches the update limit but has not yet been updated to a better position, the nectar source is discarded, and the mercenary bee corresponding to the nectar source is transformed into a scout bee. The scout bee generates a new nectar source in the search space according to the nectar source update strategy of the mercenary bee phase.

[0033] In the simplex method stage, the fitness values ​​of all nectar sources are compared, and the nectar source with the highest fitness value is selected. The simplex method is then used to optimize this nectar source.

[0034] Furthermore, during the initialization phase, the location of the nectar source is initialized, and the fitness value of the nectar source at the initial location is calculated according to the objective function, including:

[0035] Set the population size, maximum number of iterations, and initialize the location of the nectar source using the following expression:

[0036]

[0037] in, This represents the value of honey source i in dimension j, rand represents a random number in the range [0,1], and ub and lb represent the maximum and minimum values ​​of the search space, respectively.

[0038] The fitness value of the nectar source at the initial position is calculated based on the objective function.

[0039] Furthermore, the probability of selection P s The expression is:

[0040]

[0041] Where it represents the current iteration number, and Maxit represents the maximum iteration number.

[0042] Furthermore, by alternating between the refraction-based reverse learning strategy and the Cauchy mutation strategy to update the nectar source location, the update expression is:

[0043]

[0044] in, This indicates the position of nectar source i in dimension j, and b represents the adjustment coefficient. This represents the value of nectar source i in dimension j. express The inverse solution, rand represents a random number in the range [0,1], and Cauchy(·) represents the standard Cauchy distribution.

[0045] Furthermore, during the observation bee phase, the observation bee selects a hired bee to follow, searches for new nectar sources near the hired bee, and calculates the fitness value of each new nectar source to select the nectar source, including:

[0046] The observation bee selects hired bees according to the roulette strategy, and the selection expression is:

[0047]

[0048] Among them, fit i The fitness value of the nectar source is represented by SN, which represents the population size.

[0049] By observing bees to find new nectar sources, the expression for a new nectar source is:

[0050]

[0051] Among them, newV i Indicates new honey source i, V i Indicates the original nectar source i, where i ≠ k;

[0052] The fitness value of each new nectar source is calculated based on the objective function;

[0053] Furthermore, in the simplex method stage, the fitness values ​​of all nectar sources are compared, and the nectar source with the highest fitness value is selected. The simplex method is then used to optimize this nectar source, including:

[0054] Choose the nectar source with the worst adaptability. w The most adaptable nectar source x b Suboptimal honey source x t The fitness values ​​are: g(x) w ), g(x b ), g(x t );

[0055] Utilizing the optimal nectar source x b With the second best adaptability nectar source x t The center point is calculated as: x c =(x b +x t ) / 2;

[0056] Using the center point x c And the worst adaptability nectar source x w The reflection point is calculated as: x r=x c +λ(x c -x w ), where λ is the reflection coefficient;

[0057] This honey source was optimized using the simplex method:

[0058] When g(x) r ) <g(x b Perform an expansion operation to obtain the expansion point: x e =x c +β(x r -x c ), where β is the expansion coefficient;

[0059] If g(x) e ) <g(x r Expansion point x e Replace x w Otherwise, the reflection point x r Replace x w ;

[0060] When g(x) b ) <g(x r ) <g(x t ), x b x t x r Remain unchanged;

[0061] When g(x) t ) <g(x r ) <g(x w To perform a contraction operation, the contraction point is: x. cs =x c -σ(x w -x t ), σ is the point of contraction, and the point of contraction is x. cs Replace x w ;

[0062] When g(x) r )>g(x w Compression operation is performed, compression point: x cz =x c -γ(x r -x c ), where γ is the compressibility factor, and the compression point x is used. cz Replace x w ;

[0063] After replacing the worst solution with the optimal solution obtained by the simplex method, the fitness values ​​of all nectar sources are calculated to obtain the optimal solution for the population.

[0064] The above-described solution of the present invention has the following beneficial effects:

[0065] This invention obtains the forward kinematics equations by modeling the target redundant robotic arm using forward kinematics. Based on the relative positional relationship between the obstacle and each link of the target redundant robotic arm, the minimum distance between the obstacle and each link of the target redundant robotic arm is calculated, and an obstacle avoidance penalty function is established based on the minimum distance. The obstacle avoidance trajectory tracking problem of the target redundant robotic arm is transformed into a constrained optimization problem, and an objective function is established based on the desired trajectory, the forward kinematics equations, and the obstacle avoidance penalty function. The artificial bee colony algorithm is improved by integrating refraction back learning, Cauchy mutation strategy, and simplex method, resulting in an improved artificial bee colony algorithm that enhances the algorithm's local search capability and convergence speed. Based on the improved artificial bee colony algorithm, the optimal solution of the objective function under the constraints is searched, and a set of joint trajectories corresponding to the optimal solution is used as the obstacle avoidance trajectory of the target redundant robotic arm. Compared with existing technologies, this invention transforms the obstacle avoidance trajectory tracking problem of a redundant target robotic arm into a constrained optimization problem. Based on the geometric structure and relative position of the redundant target robotic arm and obstacles, an obstacle avoidance penalty function is established and introduced into the objective function to achieve active obstacle avoidance. Finally, an improved artificial bee colony algorithm is used to solve this constrained optimization problem, enabling the redundant target robotic arm to avoid obstacles during the tracking task. This simplifies the calculation process of obstacle avoidance trajectory tracking for redundant target robotic arms and ensures the real-time requirements of such tasks.

[0066] Other beneficial effects of the present invention will be described in detail in the following detailed description section. Attached Figure Description

[0067] Figure 1 This is a flowchart illustrating an embodiment of the present invention;

[0068] Figure 2 This is a schematic diagram illustrating the principle of minimum distance between the link and the obstacle in an embodiment of the present invention;

[0069] Figure 3 This is a schematic diagram illustrating the principle of the refraction-back learning strategy for generating reverse positions in an embodiment of the present invention.

[0070] Figure 4 This is a schematic diagram illustrating the principle of using the simplex method to optimize the worst solution in an embodiment of the present invention.

[0071] Figure 5 This is a flowchart of the improved artificial bee colony algorithm in an embodiment of the present invention;

[0072] Figure 6 This is a schematic diagram of the linkage of the KINOVA GEN2 six-degree-of-freedom target redundant robotic arm in an embodiment of the present invention;

[0073] Figure 7 The image shows the simulation results of the obstacle avoidance trajectory of the target redundant robotic arm in this embodiment of the invention. Detailed Implementation

[0074] To make the technical problems, solutions, and advantages of this invention clearer, a detailed description will be provided below with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0075] In the description of this invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and for simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0076] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a locking connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0077] Furthermore, the technical features involved in the different embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0078] This invention addresses existing problems by providing a kinematically-based redundant robotic arm obstacle avoidance trajectory tracking method.

[0079] like Figure 1 As shown, an embodiment of the present invention provides a kinematics-based redundant robotic arm obstacle avoidance trajectory tracking method, including:

[0080] Step 1: Perform forward kinematics modeling on the target redundant robotic arm to obtain the forward kinematics equations;

[0081] Step 2: Model the obstacle as a sphere, and model each target redundant robotic arm link and adjacent joint as a capsule. Based on the relative positional relationship between the obstacle and each capsule, calculate the minimum distance between the obstacle and each link of the target redundant robotic arm, and establish an obstacle avoidance penalty function based on the minimum distance.

[0082] Step 3: Transform the obstacle avoidance trajectory tracking problem of the target redundant robot arm into a constrained optimization problem. Establish the objective function and constraints based on the desired trajectory of the target redundant robot arm, the forward kinematic equation, and the obstacle avoidance penalty function.

[0083] Step 4: Improve the artificial bee colony algorithm by integrating refraction reverse learning, Cauchy mutation strategy and simplex method to obtain the improved artificial bee colony algorithm;

[0084] Step 5: Based on the improved artificial bee colony algorithm, search for the optimal solution of the objective function under the constraints, and use a set of joint trajectories corresponding to the optimal solution as the obstacle avoidance trajectory of the target redundant robot arm. The obstacle avoidance trajectory of the target redundant robot arm is used to input the robot model to control the target redundant robot arm of the robot model to complete the target task, such as material gripping and placing.

[0085] Specifically, step 1 includes:

[0086] By performing forward kinematic modeling on the target redundant robotic arm, the forward kinematic equations are obtained as follows:

[0087] X ee =f ee (θ)

[0088] Among them, X ee Indicates the pose of the end effector, f ee (·) represents the forward kinematics equation of the end effector, and θ represents a set of joint angles of the target redundant manipulator (the solution of the forward kinematics equation).

[0089] In this embodiment of the invention, the target redundant manipulator is modeled using the standard DH method. The standard DH method involves coinciding the link coordinate system with the base coordinate system and determining the DH parameters of the target redundant manipulator by determining the relative relationship between the link coordinate system and the base coordinate system. The DH parameters include link length a, link offset d, joint angle θ, and link twist α, and further, the homogeneous transformation matrix is ​​obtained.

[0090] It should be noted that the target redundant manipulator selected in this embodiment of the invention is the KINOWAGEN2 six-DOF target redundant manipulator, and the DH parameters are shown in Table 1 below:

[0091] Table 1. DH parameters of the KINOVA GEN2 six-DOF target redundant manipulator

[0092]

[0093]

[0094] Specifically, such as Figure 2As shown, based on the relative positional relationship between the obstacle and each capsule, the minimum distance between the obstacle and each link of the redundant robotic arm is calculated, and an obstacle avoidance penalty function is established based on the minimum distance, including:

[0095] When the center of the obstacle is O ob On the axis of the capsule The projection on is located The extension line on one side, the center of the obstacle O ob The minimum distance to link i is

[0096] When the center of the obstacle is O ob On the axis of capsule body i The projection on the axis is located on the axis Between, the center of the obstacle O ob The minimum distance to link i is

[0097] When the center of the obstacle is O ob On the axis of capsule body i The projection on is located The extension line on one side, the center of the obstacle O ob The minimum distance to link i is

[0098] Based on minimum distance d i (O,θ), the minimum distance obstacle avoidance penalty function is established as follows:

[0099]

[0100] Where, φ i (·) is the penalty function, α is the penalty coefficient, and d min To minimize the obstacle avoidance distance, d min =r ob +r link +δ d r ob It is the radius of the obstacle, r link It is the radius of the connecting rod, δ d It is a safety redundancy distance.

[0101] Specifically, the obstacle avoidance trajectory tracking problem of the redundant robotic arm is transformed into a constrained optimization problem, with the objective function being:

[0102]

[0103] Subject to θ i,min ≤θ i (t)≤θ i,max

[0104] di (O,θ(t))>d min i = 1, 2, ..., n

[0105] in, Let θ represent the desired end-effector trajectory of the redundant robotic arm, and let θ represent a set of joint angles of the redundant robotic arm. i,min and θ i,max These are the minimum and maximum values ​​of the joint angle.

[0106] Specifically, the constraint is that the joint angle changes at this θ. i,min ≤θ i (t)≤θ i,max Within the range, and the minimum distance between the robotic arm link and the obstacle is greater than the minimum obstacle avoidance distance: d i (O,θ(t))>d min ,i=1,2,…,n.

[0107] By adjusting parameters such as the desired target trajectory and obstacle radius and position in the objective function, different task requirements can be met to achieve the desired objective.

[0108] Specifically, step 4 includes:

[0109] When updating the nectar source location in the artificial bee colony algorithm, a refraction inverse learning strategy, a Cauchy mutation strategy, and a simplex strategy are introduced to obtain an improved artificial bee colony algorithm.

[0110] In this embodiment of the invention, the refraction-based reverse learning strategy introduces the principle of light refraction on the basis of reverse learning. It expands the search range by generating a reverse position using the current coordinates. The principle diagram is shown below. Figure 3 As shown, by Figure 3 It can be seen that in the coordinate system, the search range of point x is (lb, ub), the Y-axis represents the convex lens, the projection point of point A onto the X-axis is point x, and the height is h. Point A is obtained by imaging through the Y-axis lens. * The height is h * ;

[0111] The formula is derived from the above analysis:

[0112]

[0113] Transforming the above formula, we get:

[0114]

[0115] Where it represents the current iteration number, Maxit represents the maximum iteration number, and ub and lb represent the maximum and minimum values ​​of the search space, respectively.

[0116] In addition, embodiments of the present invention introduce the Cauchy mutation strategy into the update of the honey source location to enhance the algorithm's ability to escape local optima.

[0117] Specifically, such as Figure 5 As shown, the improved artificial bee colony algorithm includes an initialization phase, a hired bee phase, an observation bee phase, a scout bee phase, and a simplex method phase.

[0118] During the initialization phase, the position of the honey source is initialized, and the fitness value of the honey source at the initial position is calculated according to the objective function. There are multiple honey sources, and each honey source corresponds to a set of joint angles of the target redundant robotic arm.

[0119] During the bee-hiring phase, the location of nectar sources is updated by alternating between the refraction-back learning strategy and the Cauchy mutation strategy with different probabilities. The fitness value of the nectar source after the update is calculated and compared with the fitness value of the nectar source before the update. The nectar source with the smaller fitness value is selected.

[0120] During the observation bee phase, the observation bee selects a hired bee to follow, searches for new nectar sources near the hired bee, and calculates the fitness value of each new nectar source to select the nectar source with the lower fitness value.

[0121] During the scout bee phase, when the selected nectar source reaches the update limit but has not yet been updated to a better position, the nectar source is discarded, and the mercenary bee corresponding to the nectar source is transformed into a scout bee. The scout bee generates a new nectar source in the search space according to the nectar source update strategy of the mercenary bee phase.

[0122] In the simplex method stage, the fitness values ​​of all nectar sources are compared, and the nectar source with the highest fitness value is selected. The simplex method is then used to optimize this nectar source.

[0123] Specifically, during the initialization phase, the location of the nectar source is initialized, and the fitness value of the nectar source at the initial location is calculated according to the objective function, including:

[0124] Set the population size, maximum number of iterations, and initialize the location of the nectar source using the following expression:

[0125]

[0126] in, This represents the value of honey source i in dimension j, rand represents a random number in the range [0,1], and ub and lb represent the maximum and minimum values ​​of the search space, respectively.

[0127] The fitness value of the nectar source at the initial position is calculated based on the objective function.

[0128] Specifically, during the hired bee phase, the location of nectar sources is updated by alternately using the refraction-backward learning strategy and the Cauchy mutation strategy with selected probabilities. The fitness value of the nectar source after the update is calculated, and then compared with the fitness value of the nectar source before the update, including:

[0129] First, the refraction-backward learning strategy and the Cauchy mutation strategy are used alternately with a certain selection probability P. s The expression is:

[0130]

[0131] Where it represents the current iteration number, and Maxit represents the maximum iteration number.

[0132] Then, by alternating between the refraction-backward learning strategy and the Cauchy mutation strategy with selected probabilities, new nectar sources are searched for near the original nectar sources, and the nectar source locations are updated. The update expression is:

[0133]

[0134] in, This indicates the position of nectar source i in dimension j, and b represents the adjustment coefficient. This represents the value of nectar source i in dimension j. express The inverse solution, rand represents a random number in the range [0,1], and Cauchy(·) represents the standard Cauchy distribution.

[0135] The fitness value of the updated nectar source is calculated using the objective function;

[0136] The fitness value of the updated nectar source is compared with the fitness value of the original nectar source, and the nectar source with the lowest fitness value is selected to replace the nectar source before the update (i.e., the original nectar source).

[0137] Specifically, during the observation bee phase, the observation bee selects a hired bee to follow, searches for new nectar sources near the hired bee, and calculates the fitness value of each new nectar source to select the nectar source, including:

[0138] The observation bee selects hired bees according to the roulette strategy, and the selection expression is:

[0139]

[0140] Among them, fit i The fitness value of the nectar source is represented by SN, which represents the population size.

[0141] In this embodiment of the invention, when the accumulated p iWhen the number is greater than or equal to a random number, the observation bee selects the corresponding hired bee to search for a new nectar source. The expression for the new nectar source is:

[0142]

[0143] Among them, newV i Indicates new honey source i, V i Indicates the original nectar source i, where i ≠ k;

[0144] The fitness value of each new nectar source is calculated based on the objective function;

[0145] Based on the fitness value of each new honey source, a honey source is selected according to a greedy strategy, and each honey source corresponds to a set of joint angles of the target redundant robotic arm.

[0146] It should be noted that roulette wheel selection is a common selection strategy in optimization algorithms. Its basic idea is to use the fitness value of each individual as the size of its corresponding sector on the roulette wheel, and then determine the selected individual through random selection. The specific steps are as follows:

[0147] Calculate the fitness value of each individual and map them to an interval, which is usually [0,1].

[0148] Calculate the sum of fitness values;

[0149] Generate a random number r, in the range [0,1];

[0150] Starting with the individual with the highest fitness value, calculate the cumulative fitness value until the cumulative fitness value is greater than or equal to the random number;

[0151] Individuals with a cumulative fitness value greater than or equal to the random number are selected as the selected individuals;

[0152] Its advantage is that it can assign probabilities based on the fitness value of an individual, so that individuals with higher fitness have a greater probability of being selected, thereby improving the search efficiency of the optimization algorithm.

[0153] In this embodiment of the invention, the individual in the roulette strategy is the honey source.

[0154] It should be noted that the greedy algorithm, also known as the mountain climbing algorithm, is based on the idea of ​​gradually reaching the top of the mountain, that is, gradually obtaining the optimal solution. It is a simple but limited strategy for solving optimization problems. It can be understood as reaching the final global optimum by gradually finding local optima.

[0155] Specifically, during the scout bee phase, if a selected nectar source hasn't been updated to a better one before reaching its maximum number of updates, that nectar source is discarded, and the corresponding mercenary bee is converted into a scout bee. The scout bee then randomly generates a new nectar source in the search space and follows the formula... Update the new honey sources.

[0156] Specifically, in the simplex method stage, the fitness values ​​of all nectar sources are compared, and the nectar source with the highest fitness value is selected. The simplex method is then used to optimize this nectar source, including:

[0157] Choose the nectar source with the worst adaptability. w The most adaptable nectar source x b Suboptimal honey source x t The fitness values ​​are: g(x) w ), g(x b ), g(x t );

[0158] Utilizing the optimal nectar source x b With the second best adaptability nectar source x t The center point is calculated as: x c =(x b +x t ) / 2;

[0159] Using the center point x c And the worst adaptability nectar source x w The reflection point is calculated as: x r =x c +λ(x c -x w ), where λ is the reflection coefficient;

[0160] This honey source was optimized using the simplex method:

[0161] When g(x) r ) <g(x b Perform an expansion operation to obtain the expansion point: x e =x c +β(x r -x c ), where β represents the expansion coefficient;

[0162] If g(x) e ) <g(x r Expansion point x e Replace x w Otherwise, the reflection point x w Replace x w ;

[0163] When g(x) b) <g(x r ) <g(x t ), x b x t x r Remain unchanged;

[0164] When g(x) t ) <g(x r ) <g(x w To perform a contraction operation, the contraction point is: x. cs =x c -σ(x w -x t ), σ is the point of contraction, and the point of contraction is x. cs Replace x w ;

[0165] When g(x) r )>g(x w Compression operation is performed, compression point: x cz =x c -γ(x r -x c ), where γ is the compressibility factor, and the compression point x is used. cz Replace x w ;

[0166] After replacing the worst solution with the optimal solution obtained by the simplex method, the fitness values ​​of all nectar sources are calculated to obtain the optimal solution for the population.

[0167] To verify the effectiveness of the above method in solving the obstacle avoidance and trajectory tracking problems of redundant robotic arms, this embodiment of the invention first verifies the effectiveness of the improved artificial bee colony method using a single-point problem, and then addresses the obstacle avoidance and trajectory tracking problem. The specific process is as follows:

[0168] The parameters of the improved artificial bee colony algorithm are listed in Table 2 below:

[0169] Table 2. Parameter list of the improved artificial bee colony algorithm

[0170]

[0171] 1. Inverse kinematics solution for a single-point target redundancy robotic arm

[0172] The target point was selected as [50, 750, 200]. The comparison algorithms were PSO, ABC, and DE. The stopping condition was set to reach the maximum number of iterations, which was 200. The joint angle constraints were: -180°≤θ1≤180°, 50°≤θ2≤310°, 19°≤θ3≤340°, -180°≤θ4≤180°, -180°≤θ5≤180°, -180°≤θ6≤180°, and the population size was 100. Each algorithm was run independently 10 times to solve the obstacle avoidance trajectory tracking problem. For the four algorithms, the joint angles obtained through optimization were calculated using forward kinematics, and the maximum, minimum, average, and variance of the error accuracy were statistically obtained, as shown in Table 3.

[0173] Table 3 Comparison results of the method provided by this invention with ABC, PSO and DE algorithms

[0174] average error Maximum error Minimum error variance PSO 8.0289 12.1692 4.3609 5.2558 DE 8.6115 13.3659 3.4290 8.8182 ABC 5.2082 8.7687 2.5312 3.9663 RCSABC 0.0934 0.2778 0.0102 0.0062

[0175] As can be seen from Table 1, the average error obtained by the method provided in this embodiment of the invention is 0.0934, which is better than the results of the other three algorithms. Moreover, it is also better than PSO, ABC and DE in terms of maximum error, minimum error and variance. This result shows that RCSABC is effective in solving inverse kinematics problems.

[0176] 2. Obstacle avoidance trajectory tracking

[0177] To further investigate the performance of the method provided in the embodiments of the present invention in obstacle avoidance trajectory tracking, the parameter is set as follows: α = 10 6 The maximum number of iterations is 200, the population size is 100, and r ob =80mm, r link =40mm, δ d =5mm. Simplex method parameters: λ=1, β=2, γ=σ=0.5. The center of the circular trajectory is set to (120,750,250)mm, and the center of the obstacle is set to (150,450,260)mm and (150,250,260)mm respectively. The obstacle avoidance trajectory tracking results are as follows: Figure 7 As shown.

[0178] from Figure 7 As can be seen from (a) and 7(b), the robot end effector successfully tracked the circular trajectory even when the obstacle was in different positions. Figure 7 (c)-(f) respectively show the minimum distance between the corresponding redundant robotic arm and the obstacle, and the task space trajectory tracking error of the end effector; from Figure 7As can be seen from (a), (b), (c), and (e), the end effector of the target redundant manipulator can successfully track the expected circular trajectory. Simultaneously, the minimum distance between the target redundant manipulator and the obstacle is always greater than d. min In other words, obstacle avoidance trajectory planning was successfully achieved using the objective function and RCSABC algorithm proposed in this invention.

[0179] This invention provides an embodiment of the invention that obtains forward kinematics equations by modeling the target redundant robotic arm using forward kinematics. Based on the relative positional relationship between the obstacle and each link of the target redundant robotic arm, the minimum distance between the obstacle and each link of the target redundant robotic arm is calculated, and an obstacle avoidance penalty function is established based on the minimum distance. The obstacle avoidance trajectory tracking problem of the target redundant robotic arm is transformed into a constrained optimization problem, and an objective function is established based on the desired trajectory, forward kinematic equations, and obstacle avoidance penalty function. The artificial bee colony algorithm is improved by integrating refraction back learning, Cauchy mutation strategy, and simplex method, resulting in an improved artificial bee colony algorithm that enhances the algorithm's local search capability and convergence speed. Based on the improved artificial bee colony algorithm, the optimal solution of the objective function under the constraints is searched, and a set of joint trajectories corresponding to the optimal solution is used as the obstacle avoidance trajectory of the target redundant robotic arm. Compared with existing technologies, the embodiments of the present invention transform the obstacle avoidance trajectory tracking problem of the target redundant robotic arm into a constrained optimization problem. Based on the geometric structure and relative position of the target redundant robotic arm and the obstacle, an obstacle avoidance penalty function is established and introduced into the objective function to achieve active obstacle avoidance. Finally, the improved artificial bee colony algorithm is used to solve this constrained optimization problem, enabling the target redundant robotic arm to achieve obstacle avoidance during the tracking task. This simplifies the calculation process of obstacle avoidance trajectory tracking of the target redundant robotic arm and ensures the real-time requirements of such tasks.

[0180] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A kinematics-based redundant manipulator obstacle avoidance trajectory tracking method, characterized in that, Comprise: Step 1, the target redundant robot is kinematically modeled, and the forward kinematics equation is obtained; Step 2, the obstacle is modeled as a sphere, each target redundant robot link and adjacent joint is modeled as a capsule body, the minimum distance between the obstacle and each link of the target redundant robot is calculated according to the relative position relationship between the obstacle and each capsule body, and the obstacle avoidance penalty function is established based on the minimum distance; Step 3, the obstacle avoidance trajectory tracking problem of the target redundant robot is converted into a constrained optimization problem, the objective function and the constraint condition are established according to the desired trajectory of the target redundant robot, the forward kinematics equation and the obstacle avoidance penalty function; Step 4, the artificial bee colony algorithm is improved by fusing refraction reverse learning, Cauchy mutation strategy and simplex method, and the improved artificial bee colony algorithm is obtained; Step 5, based on the improved artificial bee colony algorithm, the optimal solution of the objective function under the constraint condition is searched, and a group of joint trajectories corresponding to the optimal solution is taken as the obstacle avoidance trajectory of the target redundant robot; Wherein, the expression of the refraction reverse learning is: ； ； wherein, denotes the current iteration number, denotes the maximum iteration number, , denote the maximum and minimum value of the search space, respectively, is the height of the point , is the height of the point , is the honey source position, and is the inverse solution of the point , denotes an intermediate parameter; The improved artificial bee colony algorithm comprises an initialization stage, an employed bee stage, an onlooker bee stage, a scout bee stage and a simplex method stage; In the employed bee phase, the selection probability is dynamically chosen The expression is: ； wherein, denotes the current iteration number, denotes the maximum iteration number; In the employed bee stage, the nectar source position is updated by alternately using the refraction reverse learning and the Cauchy mutation strategy through dynamic selection probability, and the update expression is: ； where, denotes the nectar source in the dimension of position, denotes the adjustment coefficient, , denotes the nectar source in the dimension of value, denotes the inverse solution, denotes a random number in the range denotes the standard Cauchy distribution. 2.The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 1, characterized in that, The step 1 comprises: The target redundant robot is kinematically modeled, and the forward kinematics equation is obtained as: ； wherein, denotes the end effector pose, denotes the forward kinematics equation of the end effector, denotes a set of joint angles of the target redundant robot arm, being a solution of the forward kinematics equation. 3.The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 2, characterized in that, According to the relative position relationship between the obstacle and each capsule body, the minimum distance between the obstacle and each link of the target redundant robot is calculated, and the obstacle avoidance penalty function is established based on the minimum distance, comprising: When the center of the obstacle In the capsule axis The projection on is located The extension line on one side, the center of the obstacle. to the connecting rod The minimum distance is ; When the center of the obstacle In the capsule axis The projection on the axis is located on the axis Between, the center of the obstacle to the connecting rod The minimum distance is ; When the center of the obstacle In the capsule axis The projection on is located The extension line on one side, the center of the obstacle. to the connecting rod The minimum distance is ; based on the minimum distance , the minimum distance obstacle avoidance penalty function is established as ； wherein, is a penalty function, is a penalty coefficient, is a minimum distance for obstacle avoidance, , is a radius of an obstacle, is a radius of a link, is a safety redundancy distance.

4. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 3, characterized in that, The obstacle avoidance trajectory tracking problem of the target redundant robot is converted into a constrained optimization problem, and the objective function is: ； wherein, represents a desired target redundant manipulator end-effector trajectory pose, represents a set of joint angles of the target redundant manipulator, and are minimum and maximum values of the joint angles.

5. The kinematics-based redundant robot obstacle avoidance trajectory tracking method according to claim 4, characterized in that, In the initialization stage, the positions of the nectar sources are initialized, and the fitness values of the nectar sources at the initial positions are calculated according to the objective function, the nectar sources comprise a plurality of, and each nectar source corresponds to a group of joint angles of the target redundant robot; In the employed bee stage, the nectar source positions are updated by alternately using the refraction reverse learning and the Cauchy mutation strategy through selection probability, the fitness values of the nectar sources after the position update are calculated, the fitness values of the nectar sources after the position update are compared with the fitness values of the nectar sources before the position update, and the nectar sources with smaller fitness values are selected; In the onlooker bee stage, the onlooker bees select the employed bees to follow, find new nectar sources near the employed bees, and calculate the fitness values of each new nectar source to select the nectar source; In the scout bee stage, when the selected nectar source reaches the upper limit of the update times but still has not updated to a better position, the nectar source is discarded, the employed bee corresponding to the nectar source is converted into a scout bee, and a new nectar source is generated in the search space by the scout bee according to the nectar source update strategy of the employed bee stage. In the simplex method stage, compare the fitness values of all the honey sources, select the honey source with the largest fitness value, and optimize this honey source using the simplex method. 6.The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 5, characterized in that, In the initialization stage, initialize the position of the honey source, and calculate the fitness value of the honey source at the initial position according to the optimization objective function, including: Set the population size, the maximum number of iterations, and initialize the position of the honey source, with the expression being: ； wherein, represents a honey source in the dimension of values, represents a random number in the range , respectively represent the maximum and minimum values of the search space; Calculate the fitness value of the honey source at the initial position according to the objective function. 7.The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 1, characterized in that, In the scout bee stage, the scout bee selects a forager bee to follow, finds a new honey source near the forager bee, and calculates the fitness value of each new honey source to select a honey source, including: The scout bee selects a forager bee according to the roulette strategy, with the selection expression being: ； wherein, represents the fitness value of the honey source, represents the population number; Use the scout bee to find a new honey source, with the expression of the new honey source being: ； wherein represents a new honey source , represents an original honey source , ; Calculate the fitness value of each new honey source according to the objective function. 8.The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 7, characterized in that, In the simplex method stage, compare the fitness values of all the honey sources, select the honey source with the largest fitness value, and optimize this honey source using the simplex method, including: selecting the worst fitness hives , selecting the best fitness hives , selecting the suboptimal fitness hives , the fitness values are respectively: , , ; using the fitness optimal honey source with the fitness suboptimal honey source the center point is calculated: ; Using the center point and the worst fitness of the hive The reflection point is calculated as: , is the reflection coefficient; Optimize this honey source using the simplex method: When , the expansion operation is performed to obtain an expansion point: , ; If , the expansion point is replaced by , else the reflection point is replaced by ; When , , , remain unchanged; When , the shrink operation is performed, and the shrink point: , is the shrink point, and the shrink point is replaced by ; When , the compression operation is performed, and the compression point: , is the compression coefficient, and the compression point is replaced by ; After replacing the worst solution with the optimization solution of the simplex method, calculate the fitness values of all the honey sources to obtain the optimal solution of the population.

Images