Redundant mechanical arm obstacle avoidance trajectory tracking method based on kinematics

Abstract

The invention provides a kinematics-based redundant mechanical arm obstacle avoidance trajectory tracking method, which comprises the following steps of: performing forward kinematics modeling on a target redundant mechanical arm to obtain a forward kinematics equation; the obstacle is modeled as a sphere, all target redundant mechanical arm connecting rods and adjacent joints are modeled as capsule bodies, then the minimum distance between the obstacle and all the target redundant mechanical arm connecting rods is calculated according to the relative position relation between the obstacle and all the capsule bodies, and an obstacle avoidance penalty function is established based on the minimum distance; the obstacle avoidance trajectory tracking problem of the target redundant mechanical arm is converted into a constraint optimization problem, and a target function and constraint conditions are established according to an expected trajectory, a forward kinematics equation and an obstacle avoidance penalty function; an improved artificial bee colony algorithm is used for searching an optimal solution meeting the target function under the constraint condition, and a set of joint tracks corresponding to the optimal solution serve as target redundant mechanical arm obstacle avoidance tracks; and obstacle avoidance is achieved in the process that the target redundant mechanical arm executes the tracking task.

Description

Technical Field

[0001] The invention relates to the technical field of industrial robots, and in particular to a kinematics-based redundant mechanical arm obstacle avoidance trajectory tracking method. Background Art

[0002] Redundant robotic arms have been widely used in some operational tasks that require high precision and safety due to their bionic structure, high flexibility and repeatability, such as polishing, assembly and palletizing in the field of intelligent manufacturing. In order to safely perform these high-precision operational tasks in complex environments, accurate trajectory tracking while avoiding obstacles is a very important issue that redundant robotic arms must consider.

[0003] The first step in the motion tracking control of redundant manipulators is to solve the inverse kinematics. Existing inverse kinematics solution methods are mainly divided into two categories: closed solution and numerical solution. The numerical solution mainly tracks the trajectory through the iteration of the Jacobi matrix, but the iterative principle of the numerical solution makes its solution speed much slower than the closed solution. The closed solution requires the redundant manipulator to meet the piper criterion, and has its own limitations in solving the inverse kinematics problem of redundant manipulators. Therefore, it is a new trend to use intelligent optimization algorithms to solve the inverse kinematics problem of redundant manipulators.

[0004] Traditional obstacle avoidance strategies such as artificial potential field method and Bug algorithm are designed for obstacle avoidance in Cartesian space. For the obstacle avoidance problem of redundant manipulators, additional calculations are required in the joint space, which increases the complexity of the control target. For the obstacle avoidance problem of redundant manipulators, some scholars transform obstacle avoidance into a quadratic programming problem with inequality constraints. However, this method is solved at the velocity level, requiring that the initial position of the redundant manipulator end effector must be placed on the reference trajectory, and iterative calculation of the pseudo-inverse of the Jacobian matrix is ​​required, thereby limiting the working space of the redundant manipulator and increasing the computational cost. In order to overcome these shortcomings, it is necessary to design a redundant manipulator obstacle avoidance and trajectory tracking control method at the position level. Summary of the invention

[0005] The present invention provides a redundant manipulator obstacle avoidance trajectory tracking method based on kinematics, the purpose of which is to enable a target redundant manipulator to avoid obstacles during the execution of a tracking task.

[0006] In order to achieve the above object, the present invention provides a kinematics-based redundant manipulator obstacle avoidance trajectory tracking method, comprising:

[0007] Step 1, perform forward kinematics modeling on the target redundant manipulator to obtain the forward kinematics equation;

[0008] Step 2: The obstacle is modeled as a sphere, and each target redundant robot arm link and adjacent joint are modeled as a capsule. According to the relative position relationship between the obstacle and each capsule, the minimum distance between the obstacle and each link of the target redundant robot arm is calculated, and an obstacle avoidance penalty function is established based on the minimum distance.

[0009] Step 3, transform the obstacle avoidance trajectory tracking problem of the target redundant manipulator into a constrained optimization problem, and establish the objective function and constraint conditions according to the expected trajectory of the target redundant manipulator, the forward kinematics equation and the obstacle avoidance penalty function;

[0010] Step 4, integrating refraction reverse learning, Cauchy mutation strategy and simplex method to improve the artificial bee colony algorithm, and obtaining an 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 a set of joint trajectories corresponding to the optimal solution as the target redundant robotic arm obstacle avoidance trajectory.

[0012] More specifically, step 1 includes:

[0013] The forward kinematics modeling of the target redundant manipulator is carried out, and the forward kinematics equation is obtained as follows:

[0014] X ee =f ee (θ)

[0015] Among them, X ee represents the end effector pose, 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] Specifically, according to the relative position relationship between the obstacle and each capsule, the minimum distance between the obstacle and each link of the target redundant robot arm is calculated, and an obstacle avoidance penalty function is established based on the minimum distance, including:

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

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

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

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

[0021]

[0022] Among them, φ i (·) is the penalty function, α is the penalty coefficient, d min is the minimum distance for obstacle avoidance, d min =r ob +r link +δ d , r ob is the obstacle radius, r link is the connecting rod radius, δ d is the safety redundancy distance.

[0023] Furthermore, the obstacle avoidance trajectory tracking problem of the target redundant manipulator is transformed into a constrained optimization problem, and the objective function is:

[0024]

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

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

[0027] in, represents the desired target redundant manipulator end trajectory, θ represents a set of joint angles of the target redundant manipulator, θ i,min and θ i,max are the minimum and maximum values ​​of the joint angle.

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

[0029] In the initialization stage, 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. The honey source includes multiple honey sources, and each honey source corresponds to a set of joint angles of the target redundant manipulator;

[0030] In the employed bee stage, the nectar source position is updated by alternately using the refraction reverse learning strategy and the Cauchy mutation strategy through selection probability, and the fitness value of the nectar source after the updated position is calculated. The fitness value of the nectar source after the updated position is compared with the fitness value of the nectar source before the updated position, and the nectar source with the smaller fitness value is selected;

[0031] In the observation bee stage, the observation bee selects the employed bee to follow, searches for new nectar sources near the employed bees, and calculates the fitness value of each new nectar source to select the nectar source;

[0032] In the scout bee stage, when the selected nectar source reaches the upper limit of the number of updates but still has not been updated to a better position, the nectar source is abandoned, and the employed bees corresponding to the nectar source are transformed into scout bees. A new nectar source is generated in the search space by the scout bees according to the nectar source update strategy of the employed bee stage;

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

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

[0035] Set the population size, the maximum number of iterations, and initialize the location of the nectar source. The expression is:

[0036]

[0037] in, represents the value of honey source i in dimension j, rand represents a random number in the range [0,1], 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 according to the objective function.

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

[0040]

[0041] Among them, it represents the current number of iterations, and Maxit represents the maximum number of iterations.

[0042] Furthermore, by selecting the probability to alternately use the refraction reverse learning strategy and the Cauchy mutation strategy to update the nectar source location, the update expression is:

[0043]

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

[0045] Furthermore, in the observation bee stage, the observation bee selects employed bees to follow, searches for new nectar sources near the employed bees, and calculates the fitness value of each new nectar source to select the nectar source, including:

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

[0047]

[0048] Among them, fit i represents the fitness value of the nectar source, and SN represents the population size;

[0049] Use observation bees to find new nectar sources. The expression of new nectar sources is:

[0050]

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

[0052] Calculate the fitness value of each new nectar source according to the objective function;

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

[0054] Select the nectar source with the worst fitness x w , the best nectar source x b , fitness suboptimal nectar source x t , the fitness values ​​are: g(x w ), g(x b ), g(x t );

[0055] Use the best nectar source x b With the second best nectar source x t Calculate the center point: x c =(x b +x t ) / 2;

[0056] Using the center point x c and the nectar source with the worst fitness x w Calculate the reflection point: x r=x c +λ(x c -x w ), λ is the reflection coefficient;

[0057] Use the simplex method to optimize this honey source:

[0058] When g(x r ) <g(x b ), perform expansion operation and get the expansion point: x e =x c +β(x r -x c ), β 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 ), perform shrink operation, shrink point: x cs =x c -σ(x w -x t ), σ is the contraction point, contraction point x cs Replace x w ;

[0062] When g(x r )>g(x w ), perform compression operation, compression point: x cz =x c -γ(x r -x c ), γ is the compression coefficient, and the compression point x cz Replace x w ;

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

[0064] The above scheme of the present invention has the following beneficial effects:

[0065] The present invention obtains the forward kinematics equation by performing forward kinematics modeling on the target redundant manipulator; calculates the minimum distance between the obstacle and each link of the target redundant manipulator according to the relative position relationship between the obstacle and each link of the target redundant manipulator, and establishes an obstacle avoidance penalty function based on the minimum distance; transforms the obstacle avoidance trajectory tracking problem of the target redundant manipulator into a constrained optimization problem, and establishes the objective function according to the expected trajectory, the forward kinematics equation and the obstacle avoidance penalty function; improves the artificial bee colony algorithm by integrating refractive reverse learning, Cauchy mutation strategy and simplex method, and obtains the improved artificial bee colony algorithm, which improves the local search ability and convergence speed of the algorithm; based on the improved artificial bee colony algorithm, searches for the optimal solution of the objective function that meets the constraint conditions, and uses a group of joint trajectories corresponding to the optimal solution as the obstacle avoidance trajectory of the target redundant manipulator. Compared with the prior art, the present invention transforms the obstacle avoidance trajectory tracking problem of the target redundant robotic arm into a constrained optimization problem, establishes an obstacle avoidance penalty function according to the geometric structure and relative position of the target redundant robotic arm and the obstacle, and introduces it into the objective function to achieve active obstacle avoidance, and finally uses the improved artificial bee colony algorithm to solve this constrained optimization problem, so that the target redundant robotic arm can achieve obstacle avoidance in the process of performing the tracking task, simplifies the calculation process of the obstacle avoidance trajectory tracking of the target redundant robotic arm, and ensures the real-time requirements of such tasks of the target redundant robotic arm.

[0066] Other beneficial effects of the present invention will be described in detail in the subsequent specific implementation section. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 A schematic diagram of a flow chart of an embodiment of the present invention;

[0068] Figure 2 is a schematic diagram of the minimum distance between the connecting rod and the obstacle in an embodiment of the present invention;

[0069] Figure 3 A schematic diagram of a refraction reverse learning strategy for generating a reverse position in an embodiment of the present invention;

[0070] Figure 4 A schematic diagram of the worst solution optimized by the simplex method in an embodiment of the present invention;

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

[0072] Figure 6 Schematic diagram of the KINOVA GEN2 six-degree-of-freedom target redundant manipulator connecting rod in an embodiment of the present invention;

[0073] Figure 7 This is the obstacle avoidance trajectory simulation result of the target redundant robotic arm in the embodiment of the present invention. DETAILED DESCRIPTION

[0074] In order to make the technical problems, technical solutions and advantages to be solved by the present invention clearer, the following will be described in detail with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0075] In the description of the present invention, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc., indicating the orientation or positional relationship, are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first", "second", and "third" are used for descriptive purposes only, and cannot be understood as indicating or implying relative importance.

[0076] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installation", "connection" and "connection" should be understood in a broad sense, for example, it can be a locking connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0077] In addition, 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] In view of the existing problems, the present invention provides a kinematics-based redundant manipulator obstacle avoidance trajectory tracking method.

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

[0080] Step 1, perform forward kinematics modeling on the target redundant manipulator to obtain the forward kinematics equation;

[0081] Step 2: The obstacle is modeled as a sphere, and each target redundant robot arm link and adjacent joint are modeled as a capsule. According to the relative position relationship between the obstacle and each capsule, the minimum distance between the obstacle and each link of the target redundant robot arm is calculated, and an obstacle avoidance penalty function is established based on the minimum distance.

[0082] Step 3, transform the obstacle avoidance trajectory tracking problem of the target redundant manipulator into a constrained optimization problem, and establish the objective function and constraint conditions according to the expected trajectory of the target redundant manipulator, the forward kinematics equation and the obstacle avoidance penalty function;

[0083] Step 4, integrating refraction reverse learning, Cauchy mutation strategy and simplex method to improve the artificial bee colony algorithm, and obtaining an 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 target redundant robotic arm obstacle avoidance trajectory; the target redundant robotic arm obstacle avoidance trajectory is used to input the robot model to control the target redundant robotic arm of the robot model to complete the target task, such as material grabbing and placing.

[0085] Specifically, step 1 includes:

[0086] The forward kinematics modeling of the target redundant manipulator is carried out, and the forward kinematics equation is obtained as follows:

[0087] X ee =f ee (θ)

[0088] Among them, X ee represents the end effector pose, 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 an embodiment of the present invention, forward kinematics modeling of the target redundant robotic arm is performed according to the standard DH method. The standard DH method is to overlap the link coordinate system with the base coordinate system, and obtain the DH parameters of the target redundant robotic arm by determining the relative relationship between the link coordinate system and the base coordinate system, wherein the DH parameters include link length a, link offset d, joint angle θ, link distortion α, and further obtain a homogeneous transformation matrix.

[0090] It should be noted that the embodiment of the present invention selects the target redundant manipulator as the KINOVAGEN2 six-degree-of-freedom target redundant manipulator, and the DH parameters are as shown in the following Table 1:

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

[0092]

[0093]

[0094] Specifically, if Figure 2As shown, according to the relative position 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:

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

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

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

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

[0099]

[0100] Among them, φ i (·) is the penalty function, α is the penalty coefficient, d min is the minimum distance for obstacle avoidance, d min =r ob +r link +δ d , r ob is the obstacle radius, r link is the connecting rod radius, δ d is the safety redundancy distance.

[0101] Specifically, the obstacle avoidance trajectory tracking problem of the target redundant manipulator is transformed into a constrained optimization problem, and the objective function is:

[0102]

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

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

[0105] in, represents the desired target redundant manipulator end trajectory, θ represents a set of joint angles of the target redundant manipulator, θ i,min and θ i,max are the minimum and maximum values ​​of the joint angle.

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

[0107] The embodiment of the present invention can meet different task requirements by adjusting the expected target trajectory and obstacle radius, position and other parameters in the objective function to achieve the expected goal.

[0108] Specifically, step 4 includes:

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

[0110] In the embodiment of the present invention, the refraction reverse learning strategy introduces the principle of light refraction on the basis of reverse learning, and generates reverse positions through the current coordinates to expand the search range. The principle diagram is as follows: 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 a convex lens, the projection point of point A on the X axis is point x, the height is h, and the image is formed by the Y axis lens to obtain point A. * , height h * ;

[0111] The formula obtained through the above analysis is:

[0112]

[0113] Transforming the above formula yields:

[0114]

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

[0116] In addition, the embodiment of the present invention introduces the Cauchy mutation strategy into the update of the honey source position to enhance the ability of the algorithm to jump out of the local optimal solution.

[0117] Specifically, if Figure 5 As shown, the improved artificial bee colony algorithm includes initialization stage, employed bee stage, observation bee stage, scout bee stage and simplex method stage;

[0118] In the initialization stage, 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. The honey source includes multiple honey sources, and each honey source corresponds to a set of joint angles of the target redundant manipulator;

[0119] In the employed bee stage, the nectar source position is updated by alternately using the refraction reverse learning strategy and the Cauchy mutation strategy through selection probability, and the fitness value of the nectar source after the updated position is calculated. The fitness value of the nectar source after the updated position is compared with the fitness value of the nectar source before the updated position, and the nectar source with the smaller fitness value is selected;

[0120] In the observation bee stage, the observation bee selects the employed bee to follow, searches for new nectar sources near the employed bee, and calculates the fitness value of each new nectar source to select the nectar source with a small fitness value;

[0121] In the scout bee stage, when the selected nectar source reaches the upper limit of the number of updates but still has not been updated to a better position, the nectar source is abandoned, and the employed bees corresponding to the nectar source are transformed into scout bees. A new nectar source is generated in the search space by the scout bees according to the nectar source update strategy of the employed bee stage;

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

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

[0124] Set the population size, the maximum number of iterations, and initialize the location of the nectar source. The expression is:

[0125]

[0126] in, represents the value of honey source i in dimension j, rand represents a random number in the range [0,1], 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 according to the objective function.

[0128] Specifically, in the employed bee stage, the nectar source position is updated by alternately using the refraction reverse learning strategy and the Cauchy mutation strategy through selection probability, and the fitness value of the nectar source after the updated position is calculated, and the fitness value of the nectar source after the updated position is compared with the fitness value of the nectar source before the updated position, including:

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

[0130]

[0131] Among them, it represents the current number of iterations, and Maxit represents the maximum number of iterations.

[0132] Then, by selecting the probability, the refraction reverse learning strategy and the Cauchy mutation strategy are used alternately to search for new nectar sources near the original nectar source and update the location of the nectar source. The update expression is:

[0133]

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

[0135] Calculate the fitness value of the updated nectar source through 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 smallest fitness value is selected to replace the nectar source before the update (ie, the original nectar source).

[0137] Specifically, in the observation bee stage, the observation bee selects the employed bee to follow, searches for new nectar sources near the employed bee, and calculates the fitness value of each new nectar source to select the nectar source, including:

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

[0139]

[0140] Among them, fit i represents the fitness value of the nectar source, and SN represents the population size;

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

[0142]

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

[0144] Calculate the fitness value of each new nectar source according to the objective function;

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

[0146] It should be noted that the roulette wheel selection strategy is a common selection strategy in optimization algorithms. The 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 by random selection. The specific steps are:

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

[0148] Calculate the sum of fitness values;

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

[0150] Starting from 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] Select individuals whose corresponding cumulative fitness values ​​are greater than or equal to the random number as the selected individuals;

[0152] Its advantage is that it can assign probabilities according to the fitness values ​​of individuals, so that individuals with higher fitness are more likely to be selected, thereby improving the search efficiency of the optimization algorithm.

[0153] In the embodiment of the present invention, the individuals in the roulette strategy are the nectar sources.

[0154] It should be noted that the greedy algorithm is also called the mountain climbing method. Its fundamental idea is to gradually reach the top of the mountain, that is, to gradually obtain the optimal solution. It is a simple but limited strategy for solving optimization problems. It can be understood as gradually achieving the final global optimal solution through local optimal solutions.

[0155] Specifically, in the scout bee stage, when the selected nectar source reaches the upper limit of the number of times it has not been updated and has not been updated to a better nectar source, the nectar source is abandoned and the corresponding employed bee is converted into a scout bee. The scout bee randomly generates a new nectar source in the search space and selects it according to the formula Updates to new honey sources.

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

[0157] Select the nectar source with the worst fitness x w , the best nectar source x b , fitness suboptimal nectar source x t , the fitness values ​​are: g(x w ), g(x b ), g(x t );

[0158] Use the best nectar source x b With the second best nectar source x t Calculate the center point: x c =(x b +x t ) / 2;

[0159] Using the center point x c and the nectar source with the worst fitness x w Calculate the reflection point: x r =x c +λ(x c -x w ), λ is the reflection coefficient;

[0160] Use the simplex method to optimize this honey source:

[0161] When g(x r ) <g(x b ), perform expansion operation and get the expansion point: x e =x c +β(x r -x c ), β 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 ), perform shrink operation, shrink point: x cs =x c -σ(x w -x t ), σ is the contraction point, contraction point x cs Replace x w ;

[0165] When g(x r )>g(x w ), perform compression operation, compression point: x cz =x c -γ(x r -x c ), γ is the compression coefficient, and the compression point x cz Replace x w ;

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

[0167] In order to verify the effectiveness of the above method in solving the problem of obstacle avoidance and trajectory tracking of the target redundant manipulator, the embodiment of the present invention first verifies the effectiveness of the improved artificial bee colony method from a single point problem, and then handles the obstacle avoidance trajectory tracking problem. The specific process is as follows:

[0168] First, the parameters of the improved artificial bee colony algorithm are listed as shown in Table 2:

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

[0170]

[0171] 1. Inverse kinematics solution of single-point target redundant manipulator

[0172] Select [50,750,200] as the target point, and select PSO, ABC, and DE for comparison. The stopping condition is set to reach the maximum number of iterations, the maximum number of iterations is 200, the constraint values ​​of the joint angles are: -180°≤θ1≤180°, 50°≤θ2≤310°, 19°≤θ3≤340°, -180°≤θ4≤180°, -180°≤θ5≤180°, -180°≤θ6≤180°, and the population size is 100. Each algorithm runs 10 times independently to solve the problem of obstacle avoidance trajectory tracking. For the four algorithms, the joint angles obtained by their optimization are calculated through positive kinematics, and the maximum, minimum, average, and variance of the error accuracy are statistically obtained, as shown in Table 3:

[0173] Table 3 Comparison results of the method provided by the present 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] It can be seen from Table 1 that the average error obtained by the method provided in the embodiment of the present invention is 0.0934, which is better than the results of the other three algorithms, and 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] In order to further study the performance of the method provided by the embodiment of the present invention in terms of obstacle avoidance trajectory tracking, the parameter is set as: α = 10 6 , the maximum number of iterations is 200, the population size is 100, 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 shown in Figure 7 shown.

[0178] from Figure 7 (a) and 7(b), it can be seen that the robot end-effector successfully tracks the circular trajectory when the obstacles are at different positions. Figure 7 (c)-(f) show the minimum distance between the corresponding target redundant manipulator and the obstacle and the task space trajectory tracking error of the end effector, respectively. Figure 7(a), (b), (c), and (e) show that the end effector of the target redundant manipulator can successfully track the expected circular trajectory. At the same time, the minimum distance between the target redundant manipulator and the obstacle is always greater than d min That is to say, through the objective function and RCSABC algorithm proposed in the embodiment of the present invention, the obstacle avoidance trajectory planning is successfully implemented.

[0179] The embodiment of the present invention obtains the forward kinematics equation by performing forward kinematics modeling on the target redundant robotic arm; calculates the minimum distance between the obstacle and each link of the target redundant robotic arm according to the relative position relationship between the obstacle and each link of the target redundant robotic arm, and establishes an obstacle avoidance penalty function based on the minimum distance; transforms the obstacle avoidance trajectory tracking problem of the target redundant robotic arm into a constrained optimization problem, and establishes the objective function according to the expected trajectory, the forward kinematics equation and the obstacle avoidance penalty function; improves the artificial bee colony algorithm by integrating refractive reverse learning, Cauchy mutation strategy and simplex method, and obtains the improved artificial bee colony algorithm, which improves the local search capability and convergence speed of the algorithm; based on the improved artificial bee colony algorithm, searches for the optimal solution of the objective function that meets the constraints, and uses a set of joint trajectories corresponding to the optimal solution as the obstacle avoidance trajectory of the target redundant robotic arm. Compared with the prior art, the embodiment of the present invention transforms the obstacle avoidance trajectory tracking problem of the target redundant robotic arm into a constrained optimization problem, establishes an obstacle avoidance penalty function according to the geometric structure and relative position of the target redundant robotic arm and the obstacle, and introduces it into the objective function to achieve active obstacle avoidance, and finally uses the improved artificial bee colony algorithm to solve this constrained optimization problem, so that the target redundant robotic arm can achieve obstacle avoidance in the process of performing the tracking task, simplifies the calculation process of the obstacle avoidance trajectory tracking of the target redundant robotic arm, and ensures the real-time requirements of such tasks of the target redundant robotic arm.

[0180] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A kinematics-based redundant manipulator obstacle avoidance trajectory tracking method, characterized in that: include: Step 1, perform forward kinematics modeling on the target redundant manipulator to obtain the forward kinematics equation; Step 2: Model the obstacle as a sphere, and model each target redundant robot arm link and adjacent joint as a capsule. According to the relative position relationship between the obstacle and each capsule, calculate the minimum distance between the obstacle and each link of the target redundant robot arm, and establish an obstacle avoidance penalty function based on the minimum distance. Step 3, converting the obstacle avoidance trajectory tracking problem of the target redundant manipulator into a constrained optimization problem, and establishing an objective function and constraint conditions according to the desired trajectory of the target redundant manipulator, the forward kinematics equation and the obstacle avoidance penalty function; Step 4, integrating refraction reverse learning, Cauchy mutation strategy and simplex method to improve the artificial bee colony algorithm, and obtaining an improved artificial bee colony algorithm; 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 target redundant manipulator obstacle avoidance trajectory.

2. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 1, characterized in that: The step 1 comprises: The forward kinematics modeling of the target redundant manipulator is performed, and the forward kinematics equation is obtained as follows: X ee =f ee (i) Among them, X ee represents the end effector pose, f ee (·) represents the forward kinematics equation of the end effector, θ represents a set of joint angles of the target redundant manipulator, and is the 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 manipulator is calculated, and an obstacle avoidance penalty function is established based on the minimum distance, including: When the obstacle center O ob On the axis of the capsule i The projection on The extension line of one side, the center of the obstacle circle O ob The minimum distance to connecting rod i is When the obstacle center O ob On the axis of the capsule i The projection on the axis Between, the obstacle center O ob The minimum distance to connecting rod i is When the obstacle center O ob On the axis of the capsule i The projection on The extension line of one side, the center of the obstacle circle O ob The minimum distance to connecting rod i is Based on the minimum distance d i (O,θ), the minimum distance obstacle avoidance penalty function is established as: Among them, φ i (·) is the penalty function, α is the penalty coefficient, d min is the minimum distance for obstacle avoidance, d min =r ob +r link +δ d , r ob is the radius of the obstacle, r link is the connecting rod radius, δ d is the 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 manipulator is transformed into a constrained optimization problem, and the objective function is: in, represents the desired target redundant manipulator end trajectory pose, θ represents a set of joint angles of the target redundant manipulator, θ i,min and θ i,max are the minimum and maximum values ​​of the joint angle.

5. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 4, characterized in that: The improved artificial bee colony algorithm includes an initialization stage, an employed bee stage, an observation bee stage, a scout bee stage, and a simplex method stage; In the initialization stage, 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, wherein the honey source includes multiple honey sources, and each honey source corresponds to a set of joint angles of the target redundant manipulator; In the employed bee stage, the refraction reverse learning strategy and the Cauchy mutation strategy are alternately used to update the nectar source position by selecting probability, and the fitness value of the nectar source after the position is updated is calculated, and the fitness value of the nectar source after the position is updated is compared with the fitness value of the nectar source before the position is updated, and the nectar source with a smaller fitness value is selected; In the observation bee stage, the observation bee selects employed bees to follow, searches for new nectar sources near the employed bees, and calculates the fitness value of each new nectar source to select a nectar source; In the scout bee stage, when the selected nectar source reaches the upper limit of the number of updates but still has not been updated to a better position, the nectar source is abandoned, and the employed bees corresponding to the nectar source are converted into scout bees, and a new nectar source is generated in the search space by the scout bees according to the nectar source update strategy of the employed bee stage; In the simplex method stage, the fitness values ​​of all nectar sources are compared, the nectar source with the largest fitness value is selected, and the simplex method is used to optimize this nectar source.

6. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 5, characterized in that: In the initialization stage, the position of the nectar source is initialized, and the fitness value of the nectar source at the initial position is calculated according to the optimization objective function, including: Set the population size, the maximum number of iterations, and initialize the location of the nectar source. The expression is: in, represents the value of honey source i in dimension j, rand represents a random number in the range [0,1], ub and lb represent the maximum and minimum values ​​of the search space respectively; The fitness value of the nectar source at the initial position is calculated according to the objective function.

7. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 6, characterized in that: In the employed bee stage, the dynamic selection probability P s The expression is: Among them, it represents the current number of iterations, and Maxit represents the maximum number of iterations.

8. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 7, characterized in that: In the employed bee stage, the nectar source location is updated by alternately using the refraction reverse learning strategy and the Cauchy mutation strategy through dynamic selection probability, and the update expression is: in, represents the position of nectar source i in dimension j, b represents the adjustment coefficient, represents the value of nectar source i in dimension j, express is the inverse solution of , where rand represents a random number in the range [0,1], and Cauchy(·) represents the standard Cauchy distribution.

9. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 8, characterized in that: In the observation bee stage, the observation bee selects an employed bee to follow, searches for a new nectar source near the employed bee, and calculates the fitness value of each new nectar source to select a nectar source, including: The observation bees select hired bees according to the roulette strategy, and the selection expression is: Among them, fit i represents the fitness value of the nectar source, and SN represents the population size; The observation bees are used to find new nectar sources. The expression of the new nectar source is: Among them, newV i Indicates new nectar source i, V i represents the original nectar source i, i≠k; The fitness value of each new nectar source is calculated according to the objective function.

10. The kinematics-based redundant manipulator obstacle avoidance trajectory tracking method according to claim 9, characterized in that: In the simplex method stage, the fitness values ​​of all nectar sources are compared, and the nectar source with the largest fitness value is selected. The simplex method is used to optimize this nectar source, including: Select the nectar source with the worst fitness x w , the best nectar source x b , fitness suboptimal nectar source x t , the fitness values ​​are: g(x w ), g(x b ), g(x t ); Using the optimal nectar source x b The second best nectar source x t Calculate the center point: x c =(x b +x t ) / 2; Using the center point x c and the nectar source with the worst fitness x w Calculate the reflection point: x r =x c +λ(x c -x w ), λ is the reflection coefficient; Use the simplex method to optimize this honey source: When g(x r ) <g(x b ), perform expansion operation and get the expansion point: x e =x c +β(x r -x c ), β is the expansion coefficient; If g(x e ) <g(x r ), expansion point x e Replace x w , otherwise, the reflection point x r Replace x w ; When g(x b ) <g(x r ) <g(x t ), x b 、x t 、x r remain unchanged; When g(x t ) <g(x r ) <g(x w ), perform shrink operation, shrink point: x cs =x c -σ(x w -x t ), σ is the contraction point, contraction point x cs Replace x w ; When g(x r )>g(x w ), perform compression operation, compression point: x cz =x c -γ(x r -x c ), γ is the compression coefficient, and the compression point x cz Replace x w ; After replacing the worst solution with the optimized solution of the simplex method, the fitness values ​​of all nectar sources are calculated to obtain the optimal solution for the population.

Images