A robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm

Abstract

The invention discloses a robot satellite trajectory planning method based on a multi-objective particle swarm optimization algorithm, comprising the following steps: constructing a robot manipulator motion model; performing trajectory secondary planning smoothing processing on the robot manipulator trajectory; iteratively updating the position and speed and performing time impact optimization processing; iteratively optimizing the particle swarm multi-objectives under kinematic constraints until the population optimal solution is obtained; and realizing time impact multi-objective optimization, the optimized joints of the space manipulator continuously and smoothly run, satisfying the kinematic constraints of the joints, and the speed and acceleration are continuous in a relatively short time, and the optimization effect on the joint impact is obvious, so the time and impact of the space manipulator trajectory can be optimized.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a robot satellite trajectory planning method based on a multi-objective particle swarm optimization algorithm. Background Art

[0002] As human beings continue to explore and develop space, the demand for satellite maintenance and debris removal continues to increase. Robotic satellite technology is a key technology for achieving on-orbit services and space attack and defense games. Robotic satellite is an applied technology satellite with robotics and artificial intelligence as the core design theory, with the characteristics of autonomous flight and variant control. The robotic arm is one of the typical payloads of robotic satellites, which is used to capture and repair space targets. The core of robotic satellite trajectory planning is to plan the position movement of the robotic arm. In order for the robotic arm to reach the physical capture position of the space target in the final stage, it is necessary to control the robotic arm to smoothly reach the capture position as quickly as possible, and finally complete the smooth capture and operation of the target. During the movement of the robotic arm, the joint impact has a great impact on the base of the space robotic arm. Reducing the joint impact is the key to reducing the jitter of the space robotic arm. Summary of the invention

[0003] In order to overcome the shortcomings of the prior art, the present invention provides a robot satellite trajectory planning method based on a multi-objective particle swarm optimization algorithm, which realizes multi-objective optimization through time impact. The optimized joints of the space robot arm run continuously and smoothly, satisfying the kinematic constraints of each joint. The speed and acceleration are continuous in a relatively short time, and the optimization effect on the joint impact is obvious. Therefore, the time and impact of the space robot arm trajectory can be optimized.

[0004] In order to achieve the above-mentioned object of the invention, the present invention adopts the following technical solutions:

[0005] The first aspect of the present application provides a robot satellite trajectory planning method based on a multi-objective particle swarm optimization algorithm, comprising the following steps:

[0006] S101, constructing a robot arm motion model;

[0007] S102, performing a trajectory secondary planning smoothing process on the robot arm trajectory;

[0008] S103, iteratively updating the position and speed, and performing time impact optimization processing;

[0009] S104, iteratively optimize the particle swarm multi-objective under kinematic constraints until the optimal solution of the swarm is obtained.

[0010] Furthermore, constructing the robot arm motion model includes the following steps:

[0011] Based on the position vector, velocity vector and angular velocity vector of the robot's center of mass, the position vector, velocity vector and angular velocity vector of the end of the robotic arm are constructed;

[0012] The momentum and angular momentum at the initial moment are set to 0, and the rigid body system constraints of the space robot are constructed according to the law of momentum conservation;

[0013] Perform matrix transformation on the rigid body system constraints of the space robot;

[0014] Based on the position vector, velocity vector, angular velocity vector of the end of the manipulator and the constraint matrix of the rigid body system of the space robot, the kinematic equation of the free-floating manipulator is constructed.

[0015] Furthermore, based on the position vector, velocity vector, and angular velocity vector of the robot's center of mass, the position vector, velocity vector, and angular velocity vector of the end of the robotic arm are constructed, and their expressions are as follows:

[0016]

[0017] Among them, r0, v0, ω0 are the position vector, velocity vector, and angular velocity vector of the robot's center of mass, b0 is the position vector of the robot arm joint 1 relative to the base, and a i is the position vector of the i-th link relative to joint i, b i is the position vector of joint i+1 relative to link i, k i is the rotation unit vector of the i-th joint, r i is the position vector of the i-th link, and the rotation angle of the i-th joint is r e 、v e ,ω e are the position vector, velocity vector, and angular velocity vector of the end of the robot’s manipulator, and o0 is the center of mass of the robot.

[0018] Furthermore, the momentum and angular momentum at the initial moment are set to 0, and the space robot rigid body system constraints are constructed according to the momentum conservation. The momentum conservation expression of the space robot rigid body system constraints is as follows:

[0019]

[0020] Among them, r0, v0, ω0 are the position vector, velocity vector, and angular velocity vector of the robot's center of mass, respectively. i is the position vector of the connecting rod i, are the inverse of the center of mass and the position vector of connecting rod i, respectively. Li 、m Ji are the masses of the base, link i, and joint i, respectively, I0, I Li ,I Jiare the rotational inertia matrices of the base, link i, and joint i, ω Li ,ω Ji are the angular velocity vectors of link i and joint i, respectively. is the derivative of the position vector of the i-th joint. M is the total mass of the system in matrix form, is the conjugate transpose of the position vector of the center of mass of the system and the center of mass of the base, and the rotation angle of the i-th joint is E is the identity matrix, J Tw is the tangential moment of inertia matrix of the base, H w is the inertia tensor matrix of the base, H wφ is the inertia tensor matrix of the robot combination, P and L are the momentum and angular momentum at the initial moment respectively;

[0021] Furthermore, the space robot rigid body system constraints are transformed into a matrix, and the expression of the space robot rigid body system constraint matrix is ​​as follows:

[0022]

[0023] Where M is the total mass of the system in matrix form, is the conjugate transpose of the position vectors of the system center of mass and the base center of mass; is the combined angular velocity, H w is the inertia tensor matrix of the base, H wφ is the inertia tensor matrix of the robot combination, E is the unit matrix, P and L are the momentum and angular momentum at the initial moment respectively; J Tw is the tangential moment of inertia matrix of the base, v0 and ω0 are the velocity vector and angular velocity vector of the robot’s center of mass, respectively.

[0024] Furthermore, based on the position vector, velocity vector, angular velocity vector of the end of the manipulator and the constraint matrix of the rigid body system of the space robot, the kinematic equation of the free-floating manipulator is constructed, and its expression is as follows:

[0025]

[0026] Among them, J s , J m are the Jacobian matrices of the base and the robot arm, is the conjugate transpose of the position vector of the end effector of the robot relative to the base, v0 and ω0 are the velocity vector and angular velocity vector of the robot’s center of mass, respectively. is the joint angular velocity, E is the unit matrix, v e ,ω e are the velocity vector and angular velocity vector of the robot’s end arm, R 6×7 Indicates that it is a 6×7 matrix.

[0027] Furthermore, the trajectory of the robot arm is smoothed by secondary planning, including the following steps:

[0028] Perform trajectory secondary planning based on the robot arm trajectory;

[0029] Derivative processing is performed on the trajectory quadratic programming function to obtain the function of velocity, acceleration, and jerk changing with time;

[0030] Using jerk as the optimization index, the optimal impact trajectory of the j-th joint is planned and processed;

[0031] The trajectory between the starting point and the end point of the robot arm joint space is planned and divided, and polynomial interpolation is performed to smooth the joint space trajectory.

[0032] Furthermore, the trajectory secondary planning processing is performed based on the robot arm trajectory, and its expression is as follows:

[0033]

[0034] Among them, p0, p1, …, p n is the polynomial parameter, p i is the trajectory, t i is time, and f(t) is a function with t as its parameter.

[0035] Furthermore, the trajectory quadratic programming function is differentiated to obtain the function of velocity, acceleration, and jerk changing with time, and its expression is as follows:

[0036]

[0037] Where p=[p0 p1 … p n ] T , vel(t) is the trajectory velocity function f (1) (t), acc(t) is the trajectory acceleration function f (2) (t), jerk(t) is the trajectory third-order function f (3) (t), t is the time parameter and n is the polynomial order.

[0038] Furthermore, taking jerk as the optimization index, the optimal impact trajectory of the j-th joint is planned and processed, and its expression is as follows:

[0039]

[0040] Among them, min jerk(t) j is the optimal impact trajectory of the jth joint, f 3 (t) jis the impact expression of the jth joint. f(t0) j is the starting point of the jth joint, f(t T ) j is the end position of the jth joint, f 1 (t0) j is the starting velocity of the jth joint, f 1 (t T ) j is the terminal velocity of the jth joint, f 2 (t0) j is the starting acceleration of the jth joint, f 2 (t T ) j is the terminal acceleration of the jth joint, v j (t) is the velocity of the jth joint, a j (t) is the acceleration of the jth joint. vm j ,am j are the maximum joint velocity and acceleration allowed for the jth joint, (θ0) j and (θ T ) j are the initial velocity and final velocity of the jth joint respectively.

[0041] Furthermore, planning and dividing the trajectory between the starting point and the end point of the robot arm joint space includes: dividing the trajectory planning between the starting point and the end point of the robot arm joint space into three parts, the first section uses quintic polynomial interpolation, the second section uses septic polynomial interpolation, and the third section uses quintic polynomial interpolation.

[0042] Furthermore, the expression for smoothing the joint space trajectory using polynomial interpolation is as follows:

[0043]

[0044] Among them, the coefficient a j1i (i=0,1…,5) is the coefficient of the first segment polynomial interpolation of the jth joint, a j2k (k=0,1…,7) is the coefficient of the second segment polynomial interpolation of the jth joint, a j3m (m=0,1,…,5) is the coefficient of the third segment polynomial interpolation of the jth joint, h j1 、h j2 、h j3 are the three position angles corresponding to the jth joint, and t1, t2, and t3 are the times of the first, second, and third segments respectively.

[0045] Furthermore, the position and speed are iteratively updated and time impact optimization processing is performed, which includes the following steps:

[0046] Iteratively update the position and velocity of the d-dimensional component of the i-th particle;

[0047] By combining the optimal fitness of the i-th particle, the proportion of the optimal fitness of the group, the fitness change and the number of iterations, the search speed and accuracy of the quadratic programming algorithm are improved;

[0048] Taking the maximum joint impact and running time as optimization targets, the fitness calculation is performed based on kinematic constraints.

[0049] Search for the shortest running time t by iteratively updating the expression of the jth joint and the position and velocity of the dth dimension component of the i particle i1 ,t i2 ,t i3 And perform quadratic programming on each period of time to obtain the minimum impact and shortest time of the robot arm.

[0050] Furthermore, the position and velocity of the d-dimensional component of the i-th particle are iteratively updated, and the expressions are as follows:

[0051]

[0052] in, is the k+1th iteration of the position of the d-dimensional component of the i-th particle, is the kth iteration of the position of the d-dimensional component of the i-th particle, is the kth iteration of the velocity of the d-dimensional component of the i-th particle, is the k+1th iteration of the velocity of the d-dimensional component of the i-th particle, w is the inertia weight, c1 is the particle's own velocity learning factor, p id is the optimal position of the particle, c2 is the group velocity learning factor, r1 and r2 are random numbers, p gd is the optimal position for the population.

[0053] Furthermore, by combining the optimal fitness of the i-th particle, the proportion of the optimal fitness of the group, the fitness change and the number of iterations, the search speed and accuracy of the quadratic programming algorithm are improved. The expression is as follows:

[0054]

[0055]

[0056] where w max =0.9, w min =0.3k is the current iteration number of the particle swarm, N max The maximum number of iterations set for the algorithm, pgf max is the maximum fitness set, pgf kis the optimal fitness of the group at the kth iteration, and the parameters related to the learning factor are set to c 1f =c 2f =0.05, c 1i =c 2i = 0.02. The required inertia weight w, learning factors c1 and c2 are obtained.

[0057] Furthermore, the maximum joint impact and running time are taken as optimization targets, and fitness calculation processing is performed based on kinematic constraints. The expression of fitness calculation processing is as follows:

[0058]

[0059] V imin ≤|V i |≤V imax

[0060] a imin ≤|a i |≤a imax

[0061] Among them, V i 、a i are the real-time velocity and acceleration of the i-th joint, V imin 、V imax 、a imin 、a imax is the minimum and maximum velocity and acceleration allowed for the i-th joint, f1 is fitness function 1, f2 is fitness function 2, t i1 ,t i2 ,t i3 are the time of the first, second and third segments respectively, t i is the time parameter of the i-th joint.

[0062] Furthermore, the particle swarm multi-objective is iteratively optimized under kinematic constraints until the optimal solution of the swarm is obtained, which includes the following steps:

[0063] S41, determine the size D of the initial population, and obtain the initial population and initial speed of the particle swarm algorithm according to the change in the angle of the robot arm joint i;

[0064] S42, associate the robot arm movement time and joint impact coding with time, after m groups of time t i1 ,t i2 ,t i3 Perform quadratic programming to obtain the coefficient a corresponding to each segment of the min-jerk polynomial i , where min-jerk is the minimum joint impact trajectory;

[0065] S43, the coefficient ai Substitute the polynomial interpolation into the expression for smoothing the joint space trajectory and take the derivative with respect to time to obtain the velocity and acceleration of the joint, and determine whether its maximum value satisfies the velocity and acceleration constraints of the robot joint.

[0066] S44. Filter out the results that meet the speed and acceleration constraints, calculate the fitness of its particles through the fitness formula, set the fitness of the particles that do not meet the constraints to a maximum constant, store the fitness of all initial particles in a set as the initial population, and obtain its parote solution as the optimal position of the population, where the parote solution is In multi-objective optimization, there is usually no single solution that can optimize all objectives at the same time, so it is necessary to find a set of solutions that are optimal in all objectives, but not in a single objective. This set of solutions is called the Pareto optimal solution set or the Pareto frontier, and the Pareto optimal solution is the best solution in the optimal solution set;

[0067] S45, when performing each iteration, adding each particle fitness to the particle fitness set to calculate its parote solution, and comparing it with the optimal position experienced by the particle, if the solution is the optimal position experienced by the particle, then replace it with the optimal position experienced by the particle;

[0068] S46. Assuming that the optimal position of a particle changes, replace the optimal position of the particle in the population set, and recalculate the parote solution of the population as the optimal position of the population;

[0069] S47, by continuously iteratively updating the position and velocity of the particles, a new population is obtained and steps S42 to S46 are repeated;

[0070] S48. Continuously update the population until the termination condition is met, which is the number of iterations or the optimal position.

[0071] The beneficial effects of the present application are as follows: multi-objective optimization through time impact is achieved, and the joints of the optimized space robot arm run continuously and smoothly, satisfying the kinematic constraints of each joint, and their speed and acceleration are continuous in a relatively short time, and the optimization effect on the joint impact is obvious, so the time and impact of the space robot arm trajectory can be optimized.

[0072] By constructing a space robot manipulator model, a time-impact multi-objective optimization algorithm is implemented, and the space manipulator is used to capture the process motion. The algorithm is used to optimize the running time and impact, thereby reducing the impact on the base.

[0073] For robot trajectory planning, general polynomial and spline interpolation smooth trajectories do not consider their kinematic constraints, and single-objective optimization only considers optimizing one indicator, while sacrificing some other indicators. On this basis, a robot time impact multi-objective optimization algorithm is proposed. It does not simply weight the indicators, but seeks the parote solution of its multiple objectives, which has a good effect on reducing time and joint impact.

[0074] The particle swarm algorithm is combined with the quadratic programming method, and a time-impact multi-objective optimization algorithm is proposed to search for the time-impact optimization path of the space manipulator. In order to reduce the search dimension of the time impact, the algorithm combines the combinatorial polynomial with the time impact to find a relatively optimized combination of time, and then obtains the polynomial parameters with the minimum impact through quadratic programming. In order to improve the convergence speed of the algorithm, the selection of its initial population and the inertia weight are adjusted. The algorithm is applied to the simulation of the space manipulator, and compared with other methods to verify the effectiveness of the algorithm optimization. The simulation results show that after the optimization algorithm is used, the joints of the space manipulator run continuously and smoothly, satisfying the kinematic constraints of each joint. In a relatively short time, its speed and acceleration are continuous, and the optimization effect on the joint impact is obvious. Therefore, this method can optimize the time and impact of the trajectory of the space manipulator. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0076] Figure 1 It is a schematic diagram of the steps of a robot satellite trajectory planning method based on a multi-objective particle swarm optimization algorithm of the present invention;

[0077] Figure 2 It is a schematic diagram of the solution obtained by the initial population of the present invention;

[0078] Figure 3 It is a schematic diagram of obtaining the optimal solution after repeated iterative optimization of the population of the present invention;

[0079] Figure 4 It is a schematic diagram of the joint positions of the multi-objective parote optimal solution of the present invention;

[0080] Figure 5 It is a schematic diagram of the position and velocity of each joint of the multi-objective parote optimal solution of the present invention;

[0081] Figure 6It is a schematic diagram of the acceleration of each joint position of the multi-objective parote optimal solution of the present invention;

[0082] Figure 7 It is a schematic diagram of the impact curve of each joint position of the multi-objective parote optimal solution of the present invention;

[0083] Figure 8 It is a schematic diagram of the positions of each joint of the combined polynomial algorithm of the present invention;

[0084] Fig. 9 It is a schematic diagram of the speed of each joint of the combined polynomial algorithm of the present invention;

[0085] Fig.10 It is a schematic diagram of the acceleration of each joint of the combined polynomial algorithm of the present invention;

[0086] Fig.11 It is a schematic diagram of impact curves of each joint of the combined polynomial algorithm of the present invention;

[0087] Fig.12 It is a schematic diagram of the positions of each joint with simple weighting of the optimization target of the present invention;

[0088] Fig.13 It is a schematic diagram of the simple weighted joint speed of the optimization target of the present invention;

[0089] Fig.14 It is a schematic diagram of the simple weighted acceleration of each joint according to the optimization target of the present invention;

[0090] Fig.15 It is a schematic diagram of the impact curve of each joint with simple weighting of the optimization target of the present invention. DETAILED DESCRIPTION

[0091] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0092] The following describes the embodiments of the present invention through specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the following embodiments and features in the embodiments can be combined with each other without conflict. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work belong to the scope of protection of the present invention.

[0093] Embodiment 1:

[0094] A robot satellite trajectory planning method based on a multi-objective particle swarm optimization algorithm comprises the following steps:

[0095] S101, constructing a kinematic model of the robot arm;

[0096] Building a kinematic model of the robot manipulator includes the following steps:

[0097] Based on the position vector, velocity vector and angular velocity vector of the robot's center of mass, the position vector, velocity vector and angular velocity vector of the end of the robotic arm are constructed;

[0098] The momentum and angular momentum at the initial moment are set to 0, and the rigid body system constraints of the space robot are constructed according to the law of momentum conservation;

[0099] Perform matrix transformation on the rigid body system constraints of the space robot;

[0100] Based on the position vector, velocity vector, angular velocity vector of the end of the manipulator and the constraint matrix of the rigid body system of the space robot, the kinematic equation of the free-floating manipulator is constructed.

[0101] Based on the position vector, velocity vector, and angular velocity vector of the robot's center of mass, the position vector, velocity vector, and angular velocity vector of the end of the robotic arm are constructed. The expressions are as follows:

[0102]

[0103] Among them, r0, v0, ω0 are the position vector, velocity vector, and angular velocity vector of the robot's center of mass, b0 is the position vector of the robot arm joint 1 relative to the base, and a i is the position vector of the i-th link relative to joint i, b i is the position vector of joint i+1 relative to link i, k i is the rotation unit vector of the i-th joint, r i is the position vector of the i-th link, and the rotation angle of the i-th joint is r e 、v e ,ω e are the position vector, velocity vector, and angular velocity vector of the robot’s end arm, respectively. o0 is the robot’s center of mass.

[0104] Furthermore, the momentum and angular momentum at the initial moment are set to 0, and the space robot rigid body system constraints are constructed according to the momentum conservation. The momentum conservation expression of the space robot rigid body system constraints is as follows:

[0105]

[0106] Among them, r0, v0, ω0 are the position vector, velocity vector, and angular velocity vector of the robot's center of mass, respectively. i is the position vector of the connecting rod i, are the inverse of the center of mass and the position vector of connecting rod i, respectively. Li 、m Ji are the masses of the base, link i, and joint i, respectively, I0, I Li ,I Ji are the rotational inertia matrices of the base, link i, and joint i, ω Li ,ω Ji are the angular velocity vectors of link i and joint i, respectively. is the derivative of the position vector of the i-th joint. M is the total mass of the system in matrix form, is the conjugate transpose of the position vector of the center of mass of the system and the center of mass of the base, and the rotation angle of the i-th joint is E is the identity matrix, J Tw is the tangential moment of inertia matrix of the base, H w is the inertia tensor matrix of the base, H wφ is the inertia tensor matrix of the robot combination, P and L are the momentum and angular momentum at the initial moment respectively;

[0107] Furthermore, the space robot rigid body system constraints are transformed into a matrix, and the expression of the space robot rigid body system constraint matrix is ​​as follows:

[0108]

[0109] Where M is the total mass of the system in matrix form, is the conjugate transpose of the position vectors of the system center of mass and the base center of mass; is the combined angular velocity, H w is the inertia tensor matrix of the base, H wφ is the inertia tensor matrix of the robot combination, E is the unit matrix, P and L are the momentum and angular momentum at the initial moment respectively; J Tw is the tangential moment of inertia matrix of the base, v0 and ω0 are the velocity vector and angular velocity vector of the robot's center of mass, respectively. The variables are:

[0110]

[0111] in, is the conjugate transpose of the position vector of the center of mass of the connecting rod i and the base, J TLi , J RLi Respectively represent the tangential and radial moments of inertia of the robot arm, m Li is the mass of connecting rod i. w is the inertia tensor matrix of the base, H wφis the inertia tensor matrix of the robot combination, I0, I Li are the rotational inertia matrices of the base and connecting rod i, J Tw is the tangential moment of inertia matrix of the base. m Li is the mass of connecting rod i, k i is the rotation unit vector of the i-th joint. i are the position vector of connecting rod i, p i is the position vector of the i-th joint, where 0<i≤n, and n is the number of robot joints.

[0112] Based on the position vector, velocity vector, angular velocity vector of the end of the manipulator and the constraint matrix of the rigid body system of the space robot, the kinematic equation of the free-floating manipulator is constructed, and its expression is as follows:

[0113]

[0114] Among them, J s , J m are the Jacobian matrices of the base and the robot arm, is the conjugate transpose of the position vector of the end effector of the robot relative to the base, v0 and ω0 are the velocity vector and angular velocity vector of the robot’s center of mass, respectively. is the joint angular velocity, E is the unit matrix, v e ,ω e are the velocity vector and angular velocity vector of the robot’s end arm, R 6×7 Indicates that it is a 6×7 matrix.

[0115] S102, performing a trajectory secondary planning smoothing process on the robot arm trajectory;

[0116] The robot arm trajectory is smoothed by secondary planning, including the following steps:

[0117] Perform trajectory secondary planning based on the robot arm trajectory;

[0118] Derivative processing is performed on the trajectory quadratic programming function to obtain the function of velocity, acceleration, and jerk changing with time;

[0119] Using jerk as the optimization index, the optimal impact trajectory of the j-th joint is planned and processed, where jerk is the joint impact;

[0120] The trajectory between the starting point and the end point of the robot arm joint space is planned and divided, and polynomial interpolation is performed to smooth the joint space trajectory.

[0121] The trajectory is quadratically planned based on the robot arm trajectory, and its expression is as follows:

[0122]

[0123] Among them, p0, p1, …, p n is the polynomial parameter, p i is the trajectory, t i is time, and f(t) is a function with t as its parameter.

[0124] The trajectory quadratic programming function is derived to obtain the function of velocity, acceleration, and jerk changing with time, and its expression is as follows:

[0125]

[0126] Where p=[p0 p1 … p n ] T , vel(t) is the trajectory velocity function f (1) (t), acc(t) is the trajectory acceleration function f (2) (t), jerk(t) is the trajectory third-order function f (3) (t), t is the time parameter, and n is the polynomial order. It should be noted that Minimum-jerk solves the coefficient p of the trajectory under the constraints to minimize jerk. This is a constrained optimization problem. Jerk is the third-order derivative of the trajectory. Minimum-jerk requires 6 constraints on the position, velocity, and acceleration of the starting and ending points. The quintic polynomial has 6 parameters, so the minimum order of the quadratic programming polynomial is required to be quintic.

[0127] Taking jerk as the optimization index, the optimal impact trajectory of the j-th joint is planned and processed, and its expression is as follows:

[0128]

[0129] Among them, min jerk(t) j is the optimal impact trajectory of the jth joint, f 3 (t) j is the impact expression of the jth joint. f(t0) j is the starting point of the jth joint, f(t T ) j is the end position of the jth joint, f 1 (t0) j is the starting velocity of the jth joint, f 1 (t T ) j is the terminal velocity of the jth joint, f 2 (t0) j is the starting acceleration of the jth joint, f 2(t T ) j is the terminal acceleration of the jth joint, v j (t) is the velocity of the jth joint, a j (t) is the acceleration of the jth joint. vm j ,am j are the maximum joint velocity and acceleration allowed for the jth joint, (θ0) j and (θ T ) j are the initial velocity and final velocity of the jth joint respectively.

[0130] The trajectory between the starting point and the end point of the robot joint space is planned and divided, and polynomial interpolation is performed to smooth the trajectory in the joint space. The trajectory between the starting point and the end point of the robot joint space is planned and divided, including: the trajectory planning between the starting point and the end point of the robot joint space is divided into three parts, the first section uses a fifth-order polynomial interpolation, the second section uses a seventh-order polynomial interpolation, and the third section uses a fifth-order polynomial interpolation. The velocity acceleration constraints at the starting point and the end point are zero, and t is used. 1-3 Corresponding to the three parts of time h0-h1, h1-h2, and h2-h3 respectively, mixed polynomial interpolation is used to smooth the joint space trajectory, which reduces the impact on the mechanism and reduces the amount of calculation.

[0131] The expression for smoothing the joint space trajectory using polynomial interpolation is as follows:

[0132]

[0133] Among them, the coefficient a j1i (i=0,1…,5) is the coefficient of the first segment polynomial interpolation of the jth joint, a j2k (k=0,1…,7) is the coefficient of the second segment polynomial interpolation of the jth joint, a j3m (m=0,1,…,5) is the coefficient of the third segment polynomial interpolation of the jth joint, h j1 、h j2 、h j3 are the three position angles corresponding to the jth joint, and t1, t2, and t3 are the times of the first, second, and third segments respectively.

[0134] The velocity acceleration at the starting point and the end point is zero, and the velocity acceleration at the intermediate point is continuous. Once the time of each trajectory is known, the Minimum-jerk quadratic programming algorithm can be used to solve the multi-segment jerk minimum trajectory that meets the kinematic constraints.

[0135] S103, iteratively updating the position and speed, and performing time impact optimization processing;

[0136] The iterative update of position and velocity and the time impact optimization process include the following steps:

[0137] Iteratively update the position and velocity of the d-dimensional component of the i-th particle;

[0138] By combining the optimal fitness of the i-th particle, the proportion of the optimal fitness of the group, the fitness change and the number of iterations, the search speed and accuracy of the quadratic programming algorithm are improved;

[0139] Taking the maximum joint impact and running time as optimization targets, the fitness calculation is performed based on kinematic constraints.

[0140] Search for the shortest running time t by iteratively updating the expression of the jth joint and the position and velocity of the dth dimension component of the i particle i1 ,t i2 ,t i3 And perform quadratic programming on each period of time to obtain the minimum impact and shortest time of the robot arm.

[0141] The fitness of the particle is determined by the objective function being optimized. The optimal position P of the i-th particle in each iteration is i =(p i1 ,p i2 ,…,p iD ), the best position experienced by all particles is P g , in the k+1th iteration, the position and velocity of the d-dimensional component of the i-th particle are iteratively updated. The position and velocity of the d-dimensional component of the i-th particle are iteratively updated, and the expressions are as follows:

[0142]

[0143] in, is the k+1th iteration of the position of the d-dimensional component of the i-th particle, is the kth iteration of the position of the d-dimensional component of the i-th particle, is the kth iteration of the velocity of the d-dimensional component of the i-th particle, is the k+1th iteration of the velocity of the d-dimensional component of the i-th particle, w is the inertia weight, c1 is the particle's own velocity learning factor, p id is the optimal position of the particle, c2 is the group velocity learning factor, r1 and r2 are random numbers, p gd is the optimal position for the population.

[0144] The inertia weight obtained by optimal impact trajectory planning can reduce the number of times the particle group falls into local extreme values. The learning factor will affect the search speed and accuracy of the algorithm. Similarly, combining the learning factor with the number of iterations and fitness will also affect the search speed and accuracy of the algorithm. Therefore, by combining the optimal fitness of the i-th particle, the proportion of the optimal fitness of the group, the fitness change and the number of iterations, the search speed and accuracy of the quadratic programming algorithm can be improved. The expression is as follows:

[0145]

[0146] where w max =0.9, w min =0.3k is the current iteration number of the particle swarm, N max The maximum number of iterations set for the algorithm, pgf max is the maximum fitness set, pgf k is the optimal fitness of the group at the kth iteration, and the parameters related to the learning factor are set to c 1f =c 2f =0.05, c 1i =c 2i = 0.02. The required inertia weight w, learning factors c1 and c2 are obtained.

[0147] Taking the maximum joint impact and running time as optimization targets, the fitness calculation is performed based on the kinematic constraints to achieve multi-objective parote optimal trajectory optimization based on the particle swarm algorithm. The expression of fitness calculation is as follows:

[0148]

[0149] V imin ≤|V i |≤V imax

[0150] a imin ≤|a i |≤a imax

[0151] Among them, V i 、a i are the real-time velocity and acceleration of the i-th joint, V imin 、V imax 、a imin 、a imax is the minimum and maximum velocity and acceleration allowed for the i-th joint, f1 is fitness function 1, f2 is fitness function 2, t i1 ,t i2 ,t i3 are the time of the first, second and third segments respectively, t iis the time parameter of the i-th joint.

[0152] Search for the shortest running time t by iteratively updating the expression of the jth joint and the position and velocity of the dth dimension component of the i particle i1 ,t i2 ,t i3 And perform quadratic programming on each period of time to obtain the minimum impact and shortest time of the robot arm.

[0153] S104, iteratively optimizing the particle swarm multi-objective under kinematic constraints until the optimal solution of the swarm is obtained;

[0154] Iterative optimization of particle swarm multi-objective under kinematic constraints until the optimal solution of the swarm is obtained includes the following steps:

[0155] S41, determine the size D of the initial population (20 is selected in this paper), and obtain the initial population and initial speed of the particle swarm algorithm according to the change in the angle of the robot arm joint i;

[0156] S42, associate the robot arm movement time and joint impact coding with time, after m groups of time t i1 ,t i2 ,t i3 Perform quadratic programming to obtain the coefficient a corresponding to each segment of the min-jerk polynomial i , where min-jerk is the minimum joint impact trajectory;

[0157] S43, the coefficient a i Substitute the polynomial interpolation into the expression for smoothing the joint space trajectory and take the derivative with respect to time to obtain the velocity and acceleration of the joint, and determine whether its maximum value satisfies the velocity and acceleration constraints of the robot joint.

[0158] S44, filter out the results that meet the speed and acceleration constraints, calculate the fitness of the particles using the fitness formula, and set the fitness of the particles that do not meet the constraints to a maximum constant, such as Figure 2 As shown, the fitness of all initial particles is stored in a set as the initial population, and the parote solution is the optimal position of the population, where the parote solution is In multi-objective optimization, there is usually no single solution that can optimize all objectives at the same time. Therefore, it is necessary to find a set of solutions that are optimal in all objectives, but not in a single objective. This set of solutions is called the Pareto optimal solution set or the Pareto frontier, and the Pareto optimal solution is the best solution in the optimal solution set;

[0159] S45, when performing each iteration, adding each particle fitness to the particle fitness set to calculate its parote solution, and comparing it with the optimal position experienced by the particle, if the solution is the optimal position experienced by the particle, then replace it with the optimal position experienced by the particle;

[0160] S46. Assuming that the optimal position of a particle changes, replace the optimal position of the particle in the population set, and recalculate the parote solution of the population as the optimal position of the population;

[0161] S47, by continuously iteratively updating the position and velocity of the particles, a new population is obtained and steps S42 to S46 are repeated;

[0162] S48, such as Figure 3 As shown, the population is continuously updated until the termination condition is met, which is the number of iterations or the optimal position.

[0163] S49. Use the space manipulator model to verify the time impact multi-objective optimization algorithm mentioned above and compare it with the combined polynomial and simple weighted manipulator time impact optimal algorithm. Consider the motion of the space manipulator catching a ball as an example, and use the algorithm to optimize the running time and impact to reduce the impact on the base. Figure 4-7 They are respectively the angle change, angular velocity change, angular acceleration change, and impact change of the robotic arm joint after the algorithm is optimized. Figure 8-11 It is the angle change, angular velocity change, angular acceleration change, and impact change of the combined polynomial trajectory planning algorithm of the robotic arm with kinematic constraints. Figure 12-15 It is the angle change, angular velocity change, angular acceleration change, and impact change of the simple weighted manipulator time impact optimal algorithm. By comparison, it can be seen that the time impact manipulator planning algorithm based on particle swarm has a good effect on the optimization of motion time and joint impact. The combined polynomial algorithm does not consider the kinematic constraints of joint changes, and its trajectory runs smoothly, but it can be clearly found that its impact is the largest, which means that the joint loss is serious and more energy is needed to maintain the stability of the base. Compared with the simple weighting of the optimization target, the multi-objective parote solution has better results in both time and impact indicators.

[0164] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0165] The terms "first", "second" and "third" etc. in the specification of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in a sequence other than those illustrated or described herein, for example. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

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

Claims

1. A robot satellite trajectory planning method based on a multi-objective particle swarm optimization algorithm, characterized in that: The following steps are involved: S101, constructing a robot arm motion model; S102, performing a trajectory secondary planning smoothing process on the robot arm trajectory; S103, iteratively updating the position and speed, and performing time impact optimization processing; S104, iteratively optimizing the particle swarm multi-objective under kinematic constraints until the optimal solution of the swarm is obtained; The iterative updating of the position and speed and the time impact optimization process include the following steps: Iteratively update the position and velocity of the d-dimensional component of the i-th particle; By combining the optimal fitness of the i-th particle, the proportion of the optimal fitness of the group, the fitness change and the number of iterations, the search speed and accuracy of the quadratic programming algorithm are improved; Taking the maximum joint impact and running time as optimization targets, the fitness calculation is performed based on kinematic constraints. Search for the shortest running time t by iteratively updating the expression of the jth joint and the position and velocity of the dth dimension component of the i particle i1 ,t i2 ,t i3 And perform quadratic programming on each period of time to obtain the minimum impact and shortest time of the robot arm.

2. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 1 is characterized in that: The construction of the robot arm motion model comprises the following steps: Based on the position vector, velocity vector and angular velocity vector of the robot's center of mass, the position vector, velocity vector and angular velocity vector of the end of the robotic arm are constructed; The momentum and angular momentum at the initial moment are set to 0, and the rigid body system constraints of the space robot are constructed according to the conservation of momentum; Perform matrix transformation on the rigid body system constraints of the space robot; Based on the position vector, velocity vector, angular velocity vector of the end of the manipulator and the constraint matrix of the rigid body system of the space robot, the kinematic equation of the free-floating manipulator is constructed.

3. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 2 is characterized in that: The kinematic equation of the free-floating manipulator is constructed based on the position vector, velocity vector, angular velocity vector of the manipulator end and the constraint matrix of the rigid body system of the space robot. The expression is as follows: Among them, J s , J m are the Jacobian matrices of the base and the robot arm, is the conjugate transpose of the position vector of the end effector of the robot relative to the base, v0 and ω0 are the velocity vector and angular velocity vector of the robot’s center of mass, respectively. is the joint angular velocity, E is the unit matrix, v e ,ω e are the velocity vector and angular velocity vector of the robot’s end arm, R 6×7 Indicates that it is a 6×7 matrix.

4. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 1 is characterized in that: The process of performing secondary planning and smoothing processing on the trajectory of the robot arm comprises the following steps: Perform trajectory secondary planning based on the robot arm trajectory; Derivative processing is performed on the trajectory quadratic programming function to obtain the function of velocity, acceleration, and jerk changing with time; Using jerk as the optimization index, the optimal impact trajectory of the j-th joint is planned and processed, where jerk is the joint impact; The trajectory between the starting point and the end point of the robot arm joint space is planned and divided, and polynomial interpolation is performed to smooth the joint space trajectory.

5. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 4 is characterized in that: The optimal impact trajectory of the jth joint is planned using jerk as the optimization index, and its expression is as follows: Among them, min jerk(t) j is the optimal impact trajectory of the jth joint, f 3 (t) j is the impact expression of the jth joint, f(t0) j is the starting point of the jth joint, f(t T ) j is the end position of the jth joint, f 1 (t0) j is the starting velocity of the jth joint, f 1 (t T ) j is the terminal velocity of the jth joint, f 2 (t0) j is the starting acceleration of the jth joint, f 2 (t T ) j is the terminal acceleration of the jth joint, v j (t) is the velocity of the jth joint, a j (t) is the acceleration of the jth joint. vm j ,am j are the maximum joint velocity and acceleration allowed for the jth joint, (θ0) j and (θ T ) j are the initial velocity and final velocity of the jth joint respectively.

6. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 4 is characterized in that: The expression of the polynomial interpolation for smoothing the joint space trajectory is as follows: Among them, the coefficient a j1i (i=0,1,…,5) is the coefficient of the first segment polynomial interpolation of the jth joint, a j2k (k=0,1,…,7) is the coefficient of the second segment polynomial interpolation of the jth joint, a j3m (m=0,1,…,5) is the coefficient of the third segment polynomial interpolation of the jth joint, h j1 、h j2 、h j3 are the three position angles corresponding to the jth joint, and t1, t2, and t3 are the times of the first, second, and third segments respectively.

7. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 1 is characterized in that: The position and velocity of the d-dimensional component of the i-th particle are iteratively updated, and the expression is as follows: in, is the k+1th iteration of the position of the d-dimensional component of the i-th particle, is the kth iteration of the position of the d-dimensional component of the i-th particle, is the kth iteration of the velocity of the d-dimensional component of the i-th particle, is the k+1th iteration of the velocity of the d-dimensional component of the i-th particle, w is the inertia weight, c1 is the particle's own velocity learning factor, p id is the optimal position of the particle, c2 is the group velocity learning factor, r1 and r2 are random numbers, p gd is the optimal position for the population.

8. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 1 is characterized in that: The search speed and accuracy of the quadratic programming algorithm are improved by combining the optimal fitness of the i-th particle, the proportion of the optimal fitness of the group, the fitness change and the number of iterations. The expression is as follows: where w max =0.9, w min =0.3, k is the current iteration number of the particle swarm, N max The maximum number of iterations set for the algorithm, pgf max is the maximum fitness set, pgf k is the optimal fitness of the group at the kth iteration, and the parameters related to the learning factor are set to c 1f =c 2f =0.05, c 1i =c 2i =0.02, and the required inertia weight w, learning factors c1 and c2 are obtained.

9. The robot satellite trajectory planning method based on multi-objective particle swarm optimization algorithm according to claim 1 is characterized in that: The iterative optimization of the particle swarm multi-objective under kinematic constraints until the optimal solution of the swarm is obtained includes the following steps: S41, determine the size D of the initial population, and obtain the initial population and initial speed of the particle swarm algorithm according to the change in the angle of the robot arm joint i; S42, associate the robot arm movement time and joint impact coding with time, after m groups of time t i1 ,t i2 ,t i3 Perform quadratic programming to obtain the coefficient a corresponding to each segment of the min-jerk polynomial i , where min-jerk is the minimum joint impact trajectory; S43, the coefficient a i Substitute the polynomial interpolation into the expression for smoothing the joint space trajectory and take the derivative with respect to time to obtain the velocity and acceleration of the joint, and determine whether its maximum value satisfies the velocity and acceleration constraints of the robot joint. S44. Filter out the results that meet the speed and acceleration constraints, calculate the fitness of its particles through the fitness formula, set the fitness of the particles that do not meet the constraints to a maximum constant, store the fitness of all initial particles in a set as the initial population, and obtain its parote solution as the optimal position of the population, where the parote solution is In multi-objective optimization, there is usually no single solution that can optimize all objectives at the same time, so it is necessary to find a set of solutions that are optimal in all objectives, but not in a single objective. This set of solutions is called the Pareto optimal solution set or the Pareto frontier, and the Pareto optimal solution is the best solution in the optimal solution set; S45, when performing each iteration, adding each particle fitness to the particle fitness set to calculate its parote solution, and comparing it with the optimal position experienced by the particle, if the solution is the optimal position experienced by the particle, then replace it with the optimal position experienced by the particle; S46. Assuming that the optimal position of a particle changes, replace the optimal position of the particle in the population set, and recalculate the parote solution of the population as the optimal position of the population; S47, by continuously iteratively updating the position and velocity of the particles, a new population is obtained and steps S42 to S46 are repeated; S48. Continuously update the population until the termination condition is met, which is the number of iterations or the optimal position.

Images