阅读说明：本技术 一种单关节锁定失效下的空间机械臂逆运动学方法 (Inverse kinematics method of space manipulator under single-joint locking failure ) 是由 刘正雄 王世鸥 黄攀峰 马志强 刘星 于 2021-07-12 设计创作，主要内容包括：本发明涉及一种单关节锁定失效下的空间机械臂逆运动学方法,依据SSRMS机械臂和其改进机械臂在之间的特征关系,用其等效机械臂的逆运动学结果来作为算法的迭代初值,会大大减少算法收敛到最优值的时间。在计算时,在粒子群算法的基础上,增加了分群操作,模拟了生物界的社会行为,且每次迭代会在精英群和普通群之间进行交流,将适应值差的个体进行替换,在迭代后期进行子群合并操作,方法的收敛速度会得到提升,减少了算法的计算时间。在完成任务要求的前提下可以减少计算时间。且该算法是一种数值求解方法,可以类比推广到其它构型的机械臂,另外关节正常情况下机械臂逆运动学的计算也可以使用该方法,具有较强的普适性。(The invention relates to an inverse kinematics method of a space manipulator under single joint locking failure, which can greatly reduce the time for the algorithm to converge to an optimal value by taking an inverse kinematics result of an equivalent manipulator as an iteration initial value of the algorithm according to the characteristic relationship between an SSRMS manipulator and an improved manipulator thereof. During calculation, on the basis of a particle swarm algorithm, grouping operation is added, social behaviors of the biological world are simulated, communication can be performed between an elite group and a common group during each iteration, individuals with poor adaptation values are replaced, subgroup merging operation is performed at the later stage of the iteration, the convergence speed of the method can be improved, and the calculation time of the algorithm is reduced. The calculation time can be reduced on the premise of completing the task requirement. The algorithm is a numerical solving method, can be popularized to mechanical arms of other configurations in an analogy manner, and can be used for calculating the inverse kinematics of the mechanical arm under the normal condition of the joint, so that the algorithm has strong universality.)

1. A space manipulator inverse kinematics method under single-joint locking failure is characterized by comprising the following steps:

step 1, establishing a positive kinematic model of the SSRMS mechanical arm:

[n_r,o_r,a_r,p_r]＝f_r(θ_r) (2)

establishing a positive kinematics model and an inverse kinematics model of the improved manipulator after removing an offset from the SSRMS manipulator:

[n_m,o_m,a_m,p_m]＝f_m(θ_m) (4)

in the formula [ theta ]_rRepresents the 7 joint angles, [ n ] of the actual SSRMS robot arm_r,o_r,a_r,p_r]Representing the pose of the end of the actual SSRMS arm, theta_m7 joint angles, [ n ] representing improved robot arm_m,o_m,a_m,p_m]Showing the end pose of the improved mechanical arm;

the end pose [ n ] of the target to be solved_r,o_r,a_r,p_r]The inverse kinematics function formula 5 of the equivalent mechanical arm is substituted to obtain 8 groups of inverse kinematics results;

step 2, determining an optimization objective function of inverse kinematics:

the desired position of the end of the arm is p_r，The position error function of the mechanical arm is defined as:

each joint angle of the mechanical arm is theta ═ theta₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]The initial joint angle isThe change amount of each joint angle of the mechanical arm is defined as:

the solution to inverse kinematics is converted to the following least constrained optimization problem to solve:

ΔP-ΔP_max≤0 (9)

in the formula,. DELTA.P_maxIs the maximum error;

and converting the constrained optimization problem into an unconstrained optimization problem by using a penalty function for processing, and obtaining the following optimization objective function after conversion:

where f (theta) is the optimization objective function,is a penalty function, alpha is a penalty coefficient;

step 3, setting related parameters:

setting the population scale of a particle swarm optimization algorithm as m, locking a failed joint by a mechanical arm with 7 degrees of freedom, and setting the dimension of particles to be solved, namely the number of the remaining joints of the mechanical arm as 6; setting initial values of 8 particles in the population as 8 groups of inverse kinematics results corresponding to the improved mechanical arm in the step 1 respectively, and performing chaotic initialization on the initial values of the rest m-8 particles according to the following formula

θ_ij＝l_θj+rand₁(h_θj-l_θj) (12)

v_ij＝l_vj+rand₂(h_vj-l_vj) (13)

In the formula [ theta ]_ijRepresenting the j-dimensional position component, v, of the i-th particle_ijRepresenting the j-dimensional velocity component, h, of the ith particle_θj、l_θjRespectively representing the maximum and minimum of the j-th dimension position component of the particle, h_vj、l_vjRespectively representing the maximum and minimum values, rand, of the j-th dimension velocity component of the particle₁、rand₂Respectively are random numbers between 0 and 1;

and 4, step 4: dividing the population into N-8 subgroups by adopting a K-means clustering method, and taking 8 groups of inverse kinematics results of the improved mechanical arm as an initial clustering center;

and 5: selecting the particles with the minimum evaluation value of the Top% Top of each subgroup, namely rounding to get an integer, namely Z_kForming a new elite group, wherein the original common subgroup still comprises the elite particles;

step 6: the 8 ordinary subgroups and elite subgroups are respectively and independently updated by a particle swarm algorithm, and the learning object of each subgroup is the optimal particle, namely the local optimal value, of the subgroup:

v_ij(t+1)＝ωv_ij(t)+c₁r₁(p_ij(t)-θ_ij(t))+c₂r₂(g_kj(t)-θ_ij(t)) (14)

θ_ij(t+1)＝θ_ij(t)+v_ij(t+1) (15)

where t is the current evolution algebra, t_maxFor the maximum number of iterations of the algorithm, g_kj(t) represents a local optimum value (k 1 to N) of the kth subgroup, p_ij(t) is the historical optimum of the particle, ω is the inertial weight, ω ∈ [ ω [ [ ω ])_min,ω_max]，c₁、c₂Are self-learning factor and social learning factor, r₁、r₂、r₃Respectively are random numbers between 0 and 1;

after the updating of the particle position and the speed is finished, whether the current algebra t reaches the maximum iteration time t or not is judged_maxIf t is equal to t_maxIf yes, the circulation is quitted, and the inverse kinematics result of the mechanical arm is obtained;

otherwise, executing step 7;

and 7: respectively and independently carrying out differential variation evolution operation on each subgroup and elite group

θ_i′(t)＝g_k(t)+F(g_k(t)-p_i(t)) (17)

In the formula, theta_i' (t) represents a variation vector corresponding to the particle i, F is a scaling factor between 0 and 1, and the historical optimal value and the local optimal value of the subgroup of the particles are updated according to the formula 18; if the differentially mutated particle is out of range, then any one of the modified arms 8 set of inverse solutions is set to the position of the particle;

step 8, judging whether the smart English group enters a stagnation state: f. of_max、f_minRespectively representing the maximum evaluation value and the minimum evaluation value of the particles in the elite group, det is a given precision error, if f_maxDet and f_max-f_min< det, then the subgroup is considered to enter a stagnant state, at which time each particle in the fine quartz group is altered as follows:

θ_ij＝g_kj-L_j+rand₃(2L_j) (19)

formula of middle rand₃Is a random number between 0 and 1, and beta is a scale factor;

otherwise, executing step 9;

and step 9: each normal subgroup selected Z from the elite group_kReplacing the smallest evaluation value Z in the subgroup of the particle with the particle_kA plurality of particles;

step 10, judging whether the conditions of subgroup combination are reached: calculate the variance D (g) of the local optima of 8 normal subgroups_kj) If D (g)_kj(t))≤ε、D(g_kj(t+1))≤ε、D(g_kj(t +2)) < epsilon, namely the variances of the three successive generations are less than a given error epsilon, combining all the common subgroups, and returning to the step 5 to continue updating calculation, wherein the iteration counter t is t + 1;

if the condition of subgroup merging is not met, the iteration counter t is t +1, the step 4 is returned, and next generation grouping operation is carried out;

once the subgroup merging condition is met and the subgroups are merged, only one common group and one fine English group are remained in each subsequent generation for continuous updating and evolution until the maximum iteration times of the algorithm are reached, and the inverse kinematics result of the mechanical arm is output.

2. The inverse kinematics method of a space manipulator according to claim 1, wherein the single joint lock failure is as follows: β is 0.1.

Technical Field

The invention belongs to the field of inverse kinematics of space manipulators, and relates to an inverse kinematics method of a space manipulator under the condition of single-joint locking failure, in particular to an inverse kinematics solving method based on a particle swarm algorithm when the manipulator configuration cannot solve an analytic solution after the single-joint locking failure of the space manipulator.

Background

With the development of space application technology, space manipulators play a very important role in space, including on-track service and assembly, space debris cleaning and the like. Most of space mechanical arms are redundant mechanical arms, and compared with mechanical arms with 6 degrees of freedom, the redundant mechanical arms have great advantages in the aspects of obstacle avoidance and singularity avoidance. Space robots are expensive, are usually designed to perform as many tasks as possible, and in complex and diverse space environments, the joint actuators and sensors of the robot are likely to malfunction, making timely and effective maintenance difficult or even impossible for space robots. Therefore, the space manipulator needs to have certain fault-tolerant capability.

Taking a 7-degree-of-freedom SSRMS type mechanical arm with offset on shoulders, elbows and wrists, which is applied to an international space station, as an example, the prior document carries out systematic discussion on an inverse kinematics solution method under the condition of single joint locking failure of the SSRMS type space mechanical arm, a mechanical arm with a certain joint can be degraded into a mechanical arm with 6 degrees of freedom, when 1, 2, 6 and 7 joints are locked, the mechanical arm meets the Pieper criterion and can calculate an analytical solution of the inverse kinematics, when 3, 4 and 5 joints are locked, the mechanical arm does not meet the Pieper criterion, for the case, a method for carrying out numerical iteration by using the relation between an actual mechanical arm and an improved mechanical arm is provided, in each iteration, the calculation of iteration increment is not unique due to the multiple resolvability of the inverse solution, the selected increment is not mentioned or how to select the increment to calculate the next iteration value, this method therefore has some disadvantages in its implementation.

Disclosure of Invention

Technical problem to be solved

In order to avoid the defects of the prior art, the invention provides an inverse kinematics method of a space manipulator under the condition of single-joint locking failure, which is an inverse kinematics calculation method based on a particle swarm optimization algorithm.

Technical scheme

A space manipulator inverse kinematics method under single-joint locking failure is characterized by comprising the following steps:

step 1, establishing a positive kinematic model of the SSRMS mechanical arm:

[n_r,o_r,a_r,p_r]＝f_r(θ_r) (2)

establishing a positive kinematics model and an inverse kinematics model of the improved manipulator after removing an offset from the SSRMS manipulator:

[n_m,o_m,a_m,p_m]＝f_m(θ_m) (4)

in the formula [ theta ]_rRepresents the 7 joint angles, [ n ] of the actual SSRMS robot arm_r,o_r,a_r,p_r]Representing the pose of the end of the actual SSRMS arm, theta_m7 joint angles, [ n ] representing improved robot arm_m,o_m,a_m,p_m]Showing the end pose of the improved mechanical arm;

the end pose [ n ] of the target to be solved_r,o_r,a_r,p_r]Brought into the inverse kinematics function of the equivalent robot arm 5,8 groups of inverse kinematics results are obtained;

step 2, determining an optimization objective function of inverse kinematics:

the desired position of the end of the arm is p_r，The position error function of the mechanical arm is defined as:

each joint angle of the mechanical arm is theta ═ theta₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]The initial joint angle isThe change amount of each joint angle of the mechanical arm is defined as:

the solution to inverse kinematics is converted to the following least constrained optimization problem to solve:

ΔP-ΔP_max≤0 (9)

in the formula,. DELTA.P_maxIs the maximum error;

and converting the constrained optimization problem into an unconstrained optimization problem by using a penalty function for processing, and obtaining the following optimization objective function after conversion:

where f (theta) is the optimization objective function,is a penalty function, alpha is a penalty coefficient;

step 3, setting related parameters:

setting the population scale of a particle swarm optimization algorithm as m, locking a failed joint by a mechanical arm with 7 degrees of freedom, and setting the dimension of particles to be solved, namely the number of the remaining joints of the mechanical arm as 6; setting initial values of 8 particles in the population as 8 groups of inverse kinematics results corresponding to the improved mechanical arm in the step 1 respectively, and performing chaotic initialization on the initial values of the rest m-8 particles according to the following formula

θ_ij＝l_θj+rand₁(h_θj-l_θj) (12)

v_ij＝l_vj+rand₂(h_vj-l_vj) (13)

In the formula [ theta ]_ijRepresenting the j-dimensional position component, v, of the i-th particle_ijRepresenting the j-dimensional velocity component, h, of the ith particle_θj、l_θjRespectively representing the maximum and minimum of the j-th dimension position component of the particle, h_vj、l_vjRespectively representing the maximum and minimum values, rand, of the j-th dimension velocity component of the particle₁、rand₂Respectively are random numbers between 0 and 1;

and 4, step 4: dividing the population into N-8 subgroups by adopting a K-means clustering method, and taking 8 groups of inverse kinematics results of the improved mechanical arm as an initial clustering center;

and 5: selecting the particles with the minimum evaluation value of the Top% Top of each subgroup, namely rounding to get an integer, namely Z_kForming a new elite group, wherein the original common subgroup still comprises the elite particles;

step 6: the 8 ordinary subgroups and elite subgroups are respectively and independently updated by a particle swarm algorithm, and the learning object of each subgroup is the optimal particle, namely the local optimal value, of the subgroup:

v_ij(t+1)＝ωv_ij(t)+c₁r₁(p_ij(t)-θ_ij(t))+c₂r₂(g_kj(t)-θ_ij(t)) (14)

θ_ij(t+1)＝θ_ij(t)+v_ij(t+1) (15)

where t is the current evolution algebra, t_maxFor the maximum number of iterations of the algorithm, g_kj(t) represents a local optimum value (k 1 to N) of the kth subgroup, p_ij(t) is the historical optimum of the particle, ω is the inertial weight, ω ∈ [ ω [ [ ω ])_min,ω_max]，c₁、c₂Are self-learning factor and social learning factor, r₁、r₂、r₃Respectively are random numbers between 0 and 1;

after the updating of the particle position and the speed is finished, whether the current algebra t reaches the maximum iteration time t or not is judged_maxIf t is equal to t_maxIf yes, the circulation is quitted, and the inverse kinematics result of the mechanical arm is obtained;

otherwise, executing step 7;

and 7: respectively and independently carrying out differential variation evolution operation on each subgroup and elite group

θ_i′(t)＝g_k(t)+F(g_k(t)-p_i(t)) (17)

In the formula, theta_i' (t) represents a variation vector corresponding to the particle i, F is a scaling factor between 0 and 1, and the historical optimal value and the local optimal value of the subgroup of the particles are updated according to the formula 18; if the differentially mutated particle is out of range, then one of the modified arms 8 set of inverse solutions is optionally set as the particleA location;

step 8, judging whether the smart English group enters a stagnation state: f. of_max、f_minRespectively representing the maximum evaluation value and the minimum evaluation value of the particles in the elite group, det is a given precision error, if f_maxDet and f_max-f_min< det, then the subgroup is considered to enter a stagnant state, at which time each particle in the fine quartz group is altered as follows:

θ_ij＝g_kj-L_j+rand₃(2L_j) (19)

formula of middle rand₃Is a random number between 0 and 1, and beta is a scale factor;

otherwise, executing step 9;

and step 9: each normal subgroup selected Z from the elite group_kReplacing the smallest evaluation value Z in the subgroup of the particle with the particle_kA plurality of particles;

step 10, judging whether the conditions of subgroup combination are reached: calculate the variance D (g) of the local optima of 8 normal subgroups_kj) If D (g)_kj(t))≤ε、D(g_kj(t+1))≤ε、D(g_kj(t +2)) < epsilon, namely the variances of the three successive generations are less than a given error epsilon, combining all the common subgroups, and returning to the step 5 to continue updating calculation, wherein the iteration counter t is t + 1;

if the condition of subgroup merging is not met, the iteration counter t is t +1, the step 4 is returned, and next generation grouping operation is carried out;

once the subgroup merging condition is met and the subgroups are merged, only one common group and one fine English group are remained in each subsequent generation for continuous updating and evolution until the maximum iteration times of the algorithm are reached, and the inverse kinematics result of the mechanical arm is output.

β is 0.1.

Advantageous effects

The space manipulator inverse kinematics method under the single-joint locking failure is characterized in that on the basis of a particle swarm algorithm, grouping operation is added, a common subgroup learns excellent individuals (local optimal values) of the subgroups, an elite group learns global optimal values, social behaviors of the biological world are simulated (the subgroups learn excellent individuals of the subgroups and the excellent individuals of each subgroup lead the subgroups to evolve continuously), communication can be carried out between the elite group and the common subgroup in each iteration, the individuals with poor adaptation values are replaced, and subgroup merging operation is carried out in the later iteration stage, so that the convergence speed of the algorithm can be improved, and the calculation time of the algorithm is shortened. In addition, according to the characteristic relation between the SSRMS mechanical arm and the improved mechanical arm, the inverse kinematics result of the equivalent mechanical arm is used as the iteration initial value of the algorithm, so that the time for the algorithm to converge to the optimal value is greatly reduced. In conclusion, the algorithm can reduce the calculation time on the premise of completing the task requirement. The algorithm is a numerical solving method, can be popularized to mechanical arms of other configurations in an analogy manner, and can be used for calculating the inverse kinematics of the mechanical arm under the normal condition of the joint, so that the algorithm has strong universality.

Drawings

FIG. 1 is a flow chart of an algorithm used in the present invention

FIG. 2 is a schematic diagram of a D-H coordinate system of an SSRMS-type robot arm

FIG. 3 is a schematic diagram of the D-H coordinate system of an improved mechanical arm of the SSRMS type (locking joint 5)

Detailed Description

The invention will now be further described with reference to the following examples and drawings:

the invention provides an inverse kinematics calculation method based on a particle swarm optimization algorithm, and the overall flow is shown in figure 1. The algorithm adopts the idea of dynamic K-means clustering on the basis of a standard particle swarm algorithm, divides a particle swarm into a plurality of common subgroups, and then selects excellent individuals from the common subgroups to form a refined English group. And the diversity among later subgroups can be weakened, and at the moment, common subgroups are combined, so that the consumption of computing resources is reduced. The convergence time of the particle with the worst evaluation value in the whole population is determined to be the final convergence time of the whole particle swarm algorithm, so that the exchange operation is added between the ordinary population and the elite population to replace the particle with the worse evaluation value, further the convergence speed of the particle swarm is accelerated, and the time consumption of the algorithm is reduced.

The invention is described in further detail below with reference to the accompanying drawings, in which the following detailed description is given:

step 1, establishing a positive kinematic model of the SSRMS type mechanical arm according to the D-H parameters shown in the figure 2 and the table 1.

[n_r,o_r,a_r,p_r]＝f_r(θ_r) (2)

TABLE 1D-H parameters for practical SSRMS-type robots

The mechanical arm configuration after removing one offset from the SSRMS mechanical arm is defined as an improved mechanical arm, as shown in fig. 3, it can be seen that the first 3 joint axes of the improved mechanical arm intersect at one point and satisfy the Pieper criterion, so that there is an analytical solution for the inverse kinematics of the improved mechanical arm, and a forward kinematics model and an inverse kinematics model of the improved mechanical arm are established:

[n_m,o_m,a_m,p_m]＝f_m(θ_m) (4)

in the formula [ theta ]_rRepresents the 7 joint angles, [ n ] of a real SSRMS type robot arm_r,o_r,a_r,p_r]Representation entityEnd pose, θ, of a boundary SSRMS-type robot_m7 joint angles, [ n ] representing improved robot arm_m,o_m,a_m,p_m]Showing the end pose of the improved robotic arm.

Suppose the pose of the end of the target to be solved is [ n ]_r,o_r,a_r,p_r]Because the inverse kinematics of the improved mechanical arm has an analytic solution, the end pose of the target to be solved is brought into an inverse kinematics function of the equivalent mechanical arm, and 8 groups of inverse kinematics results can be obtained;

step 2: an optimized objective function for inverse kinematics is determined. For the multi-degree-of-freedom mechanical arm, one end pose usually corresponds to multiple sets of inverse solutions, however, only one set of inverse solutions is actually needed, and therefore various constraints need to be added to select the most appropriate set of inverse solutions. Indexes such as errors of the tail end position attitude of the mechanical arm, energy consumption in the motion process, flexibility and safety are usually considered, an optimization objective function of inverse kinematics is defined according to the constraint indexes, and only two indexes of the tail end position errors and the energy consumption are considered in the case of the invention. Let the desired position of the end of the robot arm be p_r，The position error function of the mechanical arm is defined as:

each joint angle of the mechanical arm is theta ═ theta₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]The initial joint angle isIn order to minimize the energy consumption of the mechanical arm, the variation of each joint angle of the mechanical arm is defined as:

under the condition of meeting a certain position error, the minimum energy consumption is taken as an optimization target, so that the solution of the inverse kinematics can be converted into the following minimum constraint optimization problem to solve:

ΔP-ΔP_max≤0 (9)

in the formula,. DELTA.P_maxIs the maximum error.

And converting the constrained optimization problem into an unconstrained optimization problem by using a penalty function for processing, and obtaining the following optimization objective function after conversion:

where f (theta) is an optimization objective function,is a penalty function, alpha is a penalty coefficient;

and step 3: and setting related parameters and initializing the population. The population size is m, and the particle dimension to be solved is 6 because a mechanical arm with 7 degrees of freedom locks one joint. Setting initial values of 8 particles in the population as 8 groups of inverse kinematics results corresponding to the improved mechanical arm in the step 1 respectively, and carrying out chaotic initialization on the initial values of the rest m-8 particles according to the following formula:

θ_ij＝l_θj+rand₁(h_θj-l_θj) (12)

v_ij＝l_vj+rand₂(h_vj-l_vj) (13)

in the formula [ theta ]_ijIndicates the ith granulePosition component of the child in dimension j, v_ijRepresenting the j-dimensional velocity component, h, of the ith particle_θj、l_θjRespectively representing the maximum and minimum of the j-th dimension position component of the particle, h_vj、l_vjRespectively representing the maximum and minimum values, rand, of the j-th dimension velocity component of the particle₁、rand₂Respectively are random numbers between 0 and 1;

and 4, step 4: dividing the population into N sub-populations by adopting a K-means clustering method, wherein N is 8, and taking 8 sets of inverse kinematics results of the improved mechanical arm as an initial clustering center;

and 5: selecting the particles with the minimum evaluation value of the Top% Top of each subgroup (taking an integer by rounding, namely Z)_kRespectively) forming a new elite group, wherein the original common elite group still comprises the elite particles;

step 6: the 8 normal subgroups and elite subgroups are updated by performing the particle swarm optimization independently according to the following formula, and it should be noted that the learning object of each subgroup is the optimal particle (local optimal value) of the subgroup.

v_ij(t+1)＝ωv_ij(t)+c₁r₁(p_ij(t)-θ_ij(t))+c₂r₂(g_kj(t)-θ_ij(t)) (14)

θ_ij(t+1)＝θ_ij(t)+v_ij(t+1) (15)

Where t is the current evolution algebra, t_maxFor the maximum number of iterations of the algorithm, g_kj(t) represents a local optimum value (k 1 to N) of the kth subgroup, p_ij(t) is the historical optimum of the particle, ω is the inertial weight, ω ∈ [ ω [ [ ω ])_min,ω_max]，c₁、c₂Are self-learning factor and social learning factor, r₁、r₂、r₃Are random numbers between 0 and 1.

Judging the current after the updating of the particle position and the velocity is finishedWhether the algebra t has reached the maximum number of iterations t_maxIf t is equal to t_maxAnd then the loop is exited to obtain the inverse kinematics result of the mechanical arm.

Otherwise, executing step 7;

and 7: and (3) respectively and independently carrying out differential variation evolution operation on each subgroup and the elite group:

θ_i′(t)＝g_k(t)+F(g_k(t)-p_i(t)) (17)

in the formula, theta_i' (t) represents a variation vector corresponding to the particle i, F is a scaling factor between 0 and 1, and the particle history optimal value and the subgroup local optimal value are updated according to the formula (18). If the differentially mutated particle is out of range, then selecting one from 8 sets of inverse solutions of the modified robotic arm, and setting the solution as the position of the out-of-range particle;

and 8: and judging whether the Jingying group enters a stagnation state or not. f. of_max、f_minRespectively representing the maximum evaluation value and the minimum evaluation value of the particles in the elite group, det is a given precision error, if f_maxDet and f_max-f_min< det, then the subgroup is considered to enter a stagnant state, at which time each particle in the fine quartz group is altered as follows:

θ_ij＝g_kj-L_j+rand₃(2L_j) (19)

formula of middle rand₃The beta is a random number between 0 and 1, beta is a scaling factor, and beta is 0.1;

and step 9: each normal subgroup selected Z from the elite group_kReplacing the smallest evaluation value Z in the subgroup of the particle with the particle_kA plurality of particles;

step 10: judging whether subgroup merging is reachedThe conditions of (1). Calculate the variance D (g) of the local optima of 8 normal subgroups_kj) If D (g)_kj(t))≤ε、D(g_kj(t+1))≤ε、D(g_kj(t +2)) < epsilon, that is, the variances of three consecutive generations are less than a given error epsilon, the diversity among subgroups is considered to weaken and tend to be consistent, at the moment, the influence on the result is very small by grouping and calculating, in order to save calculation resources and improve the calculation speed, all the ordinary subgroups can be combined, an iteration counter t is t +1, and the step 5 is returned to continue to update and calculate.

If the condition for merging subgroups is not satisfied, the iteration counter t is t +1, the process returns to step 4, and the next generation of grouping operation is performed.

Once the subgroup merging condition is met and the subgroups are merged, only one common group and one fine English group are remained in each subsequent generation for continuous updating and evolution until the maximum iteration times of the algorithm are reached, and the inverse kinematics result of the mechanical arm is output.

The algorithm provided by the invention is a numerical solving method, and the method can be well used for the mechanical arm of which the inverse kinematics can not obtain an analytic solution. Space manipulators are most often biased in configuration, and the present invention is exemplified by an SSRMS type manipulator. The corresponding end positions of the SSRMS mechanical arm and the improved mechanical arm of the SSRMS mechanical arm are different under the same group of joint angles, but the postures of the SSRMS mechanical arm and the improved mechanical arm are the same, so that the inverse solution result calculated by the inverse kinematics of the equivalent mechanical arm for the target end pose is very close to the inverse solution result of the actual SSRMS mechanical arm, and by utilizing the characteristics, the inverse kinematics result of the equivalent mechanical arm is used as an iteration initial value of the algorithm, so that the time for the algorithm to converge to an optimal value can be greatly reduced. The algorithm can reduce the calculation time on the premise of meeting the task requirements, can be popularized to mechanical arms of other configurations in an analogy manner, and can be used for calculating the inverse kinematics of the mechanical arm under the normal condition of the joint.