Intelligent operation track optimization method and system for autonomous manipulator of amphibious unmanned platform

Abstract

The invention provides an intelligent operation track optimization method and system for an autonomous manipulator of an amphibious unmanned platform, and belongs to the field of manipulator track planning. The problems that the continuity of the joint speed, acceleration and jerk of a mechanical arm is poor, the overall operation time is long, impact is high during movement, energy consumption is large, and the working efficiency of the mechanical arm is low are solved. Comprising the steps of mechanical arm kinematics model building, mechanical arm working space solving, trajectory planning based on 3-5-3 polynomial interpolation and multi-target trajectory planning based on an improved particle swarm algorithm. According to the method, optimization planning of the trajectory is carried out by considering optimization targets with optimal time, optimal impact and minimum energy; for the time optimization problem, the movement speed of each joint can be maximized, the whole movement time is shortened, and the working efficiency of the robot is improved; the optimal impact planning can improve the smoothness of manipulator movement, reduce the tracking error, reduce the impact during movement, improve the stability of manipulator operation and prolong the service life of the manipulator.

Description

Technical Field

[0001] The present invention relates to the technical field of manipulator trajectory planning, and in particular to a method and system for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform. Background Art

[0002] As many resources on land are gradually depleted, people are paying more and more attention to the development of marine resources. Underwater manipulators provide the necessary tools and technical support for the exploitation of marine oil and natural gas, as well as the development of seabed mineral resources. Underwater manipulators can also perform detection, sampling and observation tasks in deep-sea environments, helping scientists understand deep-sea biological, geological and physical phenomena, and promoting the development of deep-sea science. In addition, underwater manipulators play a key role in the maintenance and rescue tasks of marine engineering and underwater facilities. They are able to perform maintenance, installation and disassembly tasks, and solve difficult-to-reach or dangerous workplaces in underwater environments. Underwater manipulators are generally considered to be the most suitable tools for performing seabed intervention operations.

[0003] However, in the process of completing underwater tasks, existing manipulators have problems such as poor continuity of joint speed, acceleration and jerk, long overall operation time, unstable changes in the positions of various joints, strong impact during movement, high energy consumption and low working efficiency of the manipulator.

[0004] In addition, uncertain interferences such as waves and surges that often occur in underwater environments make it extremely difficult to accurately model underwater manipulators and lead to errors in the models. These uncertain factors pose challenges to the research on autonomous operation technology of underwater manipulators, making the research on their trajectory planning particularly important. Summary of the invention

[0005] The technical problems to be solved by the present invention are:

[0006] In order to solve the problems of poor continuity of joint speed, acceleration and jerk in existing manipulators, long overall operating time, unstable changes in the positions of joints, strong impact during movement, high energy consumption and low working efficiency of the manipulator.

[0007] The present invention adopts the following technical solutions to solve the above technical problems:

[0008] The present invention provides a method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform, comprising the following steps:

[0009] S100, establishing a manipulator motion model, including a forward motion model and an inverse motion model, wherein the forward motion model obtains the position and posture of the end effector relative to the base coordinate system through coordinate transformation, and the inverse motion model infers the value of each joint variable when the position and posture of the key points of the end effector path are known;

[0010] S200, using the Monte Carlo method to solve the workspace of the six-degree-of-freedom manipulator, and obtaining a three-dimensional point cloud image of the manipulator in the workspace and its projection on each coordinate axis plane;

[0011] S300, performing trajectory planning of the manipulator based on the 3-5-3 polynomial interpolation method, using the manipulator inverse kinematics model of step S100 to solve the path points in the joint space, and describing the function of the displacement of each joint over time under specific constraints;

[0012] S400, using an improved particle swarm algorithm, introducing dynamic adaptive adjustment of inertia weights and dynamic adjustment of learning factors, optimizes the variable trajectory of each joint of the manipulator under kinematic constraints, and obtains a multi-objective optimization trajectory that meets the optimal time-impact-energy requirements.

[0013] Furthermore, in step S100, it includes:

[0014] S110, establishing a forward kinematic model of the manipulator, and enabling the end of the manipulator to reach a specified position and posture through a given angle through the forward kinematic model of the manipulator;

[0015] Set DH parameters, including joint j, angle θ between two common vertical lines j , the distance d between the intersection points of two common perpendicular lines on the same axis j , the distance between the two shafts a j-1 , the angle between the two axes And the joint range, the change matrix of each adjacent joint is as follows:

[0016]

[0017] Where sinθ i Abbreviated as s i , cosθ i Abbreviated as c i ,sin(θ 1 +θ 2 ) is abbreviated as s 12 , cos(θ 1 +θ 2 ) is abbreviated as c 12 ;

[0018] Multiply the transformation matrices of each link in sequence to obtain the transformation matrix of the manipulator end coordinate system relative to the base coordinate system for:

[0019]

[0020] Among them, according to the matrix multiplication rule, we can get:

[0021]

[0022] Transformation matrix via link Substitute the variables of each joint to obtain the position and posture of the end of the manipulator in the spatial coordinate system.

[0023] Furthermore, in step S200, after initialization, the number of samples N=1 is set, the range of each joint is defined, the joint angles are randomly generated using a random number generator, the position of the end effector is calculated by forward motion, and it is determined whether the manipulator is in the workspace. If not, the joint angles are regenerated and the end effector position is calculated. If so, the effective position of the manipulator is recorded, and the three-dimensional point cloud map of the manipulator in the workspace and its projection on each coordinate axis plane are obtained by setting the number of generated samples N=30000.

[0024] Furthermore, in step S300, it includes:

[0025] The general formula for a 3-5-3 piecewise polynomial is:

[0026]

[0027] Where, j = 1, 2, 3, ..., 6; coefficient a j1i 、a j2i 、a j3i Represents the i-th coefficient of the polynomial interpolation function of the first, second, and third segments of the j-th joint trajectory; θ j1 (t),θ j2 (t),θ j3 (t) represent the first cubic polynomial trajectory, the second quintic polynomial trajectory and the third cubic polynomial trajectory of the joint segment, respectively;

[0028] Assume T j , J j , A j0 、V j , X j They represent the time, pulsation, acceleration, velocity and position of the j-th joint interpolation respectively. Given the initial point X of each segment of the j-th joint j0 、Waypoint X j1 、Waypoint X j2 、The target point is X j3 , the acceleration and velocity of the initial point and the target point are A j0 , A j3 、V j0 、V j3 , and the velocity and acceleration between each interpolation point are continuous. According to the above conditions, the relationship between the unknown coefficient and the interpolation point can be derived as shown in the following formula:

[0029]

[0030] b=[000000X j3 00 X j0 00 X j2 X j1 ] T

[0031] a=inv(A)*b

[0032] a=[a j13 a j12 a j11 a j10 a j25 a j24 a j23 a j22 a j21 a j20 a j33 a j32 a j31 a j30 ] T

[0033] Among them, j = 1, 2…6, representing the joints of the underwater manipulator, X j0 ~X j3 It is the running path point of the underwater manipulator; a j10 ~a j33 are the coefficients of the angle equations of each joint.

[0034] Furthermore, in step S400, it includes:

[0035] The weight adaptive adjustment formula is set as:

[0036]

[0037] Among them, the minimum weight ω min The value is 0.1, the maximum weight ω max The value is 0.9, k is the current iteration number, and N is the maximum iteration number;

[0038] Dynamically adjust the learning factor. In the early stage of the algorithm, the self-learning factor c 1 Set to be higher than the group learning factor c 2 , in the later stage of iteration, set c 1 Below c 2 , c 1 ,c 2 The constructor is as follows:

[0039]

[0040] The present invention provides a system for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform. The system has a program module corresponding to the above steps, and executes the steps in the above method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform during operation.

[0041] The present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of a method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform when called by a processor.

[0042] Compared with the prior art, the present invention has the following beneficial effects:

[0043] The present invention discloses an intelligent operation trajectory optimization method and system for an autonomous manipulator of an amphibious unmanned platform, including the establishment of a manipulator kinematic model, the solution of the manipulator workspace, the trajectory planning based on 3-5-3 polynomial interpolation, and the multi-objective trajectory planning based on an improved particle swarm algorithm; for the trajectory optimization problem of an underwater manipulator delivering a load in water, on the basis of basic trajectory planning, the trajectory optimization planning is performed by considering the optimization goals of optimal time, optimal impact, and minimum energy; wherein, for the time optimization problem, the method can maximize the movement speed of each joint to shorten the entire movement time, thereby improving the work efficiency of the robot; the impact optimization planning is to improve the smoothness of the manipulator's movement, reduce its tracking error, reduce its impact during movement, improve the stability of the manipulator's operation, and extend its service life; since a single optimization goal no longer meets the task requirements of the manipulator's underwater operation, the present invention comprehensively optimizes multiple optimization goals;

[0044] In addition, in response to the problem that the standard particle swarm algorithm is prone to fall into local optimality and has slow convergence speed in the later stage, the present invention proposes an improved particle swarm optimization method with dynamic inertia weight adjustment and adaptive adjustment of learning factor. The method is adjusted based on the number of iterations or the performance of particles; in the early stage of iteration, a larger inertia weight is set to allow particles to search in a larger range; when the algorithm is executed to the middle and late stages, the particles are close to the region of the global optimal solution. At this time, the inertia weight is reduced to allow the particles to perform a more detailed search in order to find a more accurate optimal solution; the dynamic adjustment mechanism of the learning factor reflects the coordination and adaptation between individuals and groups, ensuring the efficiency and robustness of the particle swarm optimization algorithm in solving complex problems. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a flow chart of a method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform in an embodiment of the present invention;

[0046] Figure 2The three-dimensional point cloud and plane projection diagram of the manipulator in the working space in the embodiment of the present invention, wherein (a) is the three-dimensional point cloud of the manipulator in the working space, (b) is the plane projection diagram of the manipulator on the xoy surface, (c) is the plane projection diagram of the manipulator on the xoz surface, and (d) is the plane projection diagram of the manipulator on the yoz surface;

[0047] Figure 3 1 is a graph showing the change of motion parameters of the manipulator when the interpolation time is 5s-5s-5s in an embodiment of the present invention, wherein, when the total time is the same as 15s, (a) is a graph showing the change of the manipulator angle curve, (b) is a graph showing the change of the manipulator velocity curve, (c) is a graph showing the change of the manipulator acceleration curve, and (d) is a graph showing the change of the manipulator jerk curve;

[0048] Figure 4 The figures are the motion parameter change curves of the manipulator when the interpolation time is 3s-8s-4s in the embodiment of the present invention, wherein, when the total time is the same as 15s, (a) is the manipulator angle curve change graph, (b) is the manipulator speed curve change graph, (c) is the manipulator acceleration curve change graph, and (d) is the manipulator jerk curve change graph;

[0049] Figure 5 It is a comparison diagram of iterative change curves of six joints after using standard PSO and improved PSO in an embodiment of the present invention;

[0050] Figure 6 The six-joint variable curve comparison diagram after using the standard PSO and the improved PSO in the embodiment of the present invention, wherein (a) is the position curve change diagram, velocity curve change diagram and acceleration curve change diagram after using the standard PSO, and (b) is the position curve change diagram, velocity curve change diagram and acceleration curve change diagram after using the improved PSO;

[0051] Figure 7 Flowchart of the Monte Carlo solution process.

[0052] Figure 8 The inertia weight change curve and the learning factor change curve using the improved particle swarm optimization algorithm in the embodiment of the present invention are shown in Figure 1. Under the condition that the evolutionary algebra is 100 generations, (a) is the inertia weight change curve with the evolutionary algebra, and (b) is the learning factor change curve with the evolutionary algebra. DETAILED DESCRIPTION

[0053] In the description of the present invention, it should be noted that the terms "first", "second", and "third" mentioned in the embodiments of the present invention are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first", "second", and "third" may explicitly or implicitly include one or more of the features.

[0054] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0055] Specific implementation plan 1: Combine Figures 1 to 8 As shown, the present invention provides a method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform, comprising the following steps:

[0056] S100, establishing a manipulator motion model, including a forward motion model and an inverse motion model, wherein the forward motion model derives the position and posture of the end effector relative to the base coordinate system through coordinate transformation, and the inverse motion model reversely derives the value of each joint variable when the position and posture of the key points of the end effector path are known, including:

[0057] Taking the six-degree-of-freedom serial manipulator, Huayan X5 underwater manipulator, as an example, combined with DH parameters, the mathematical model establishment method of X5 underwater manipulator and related issues of forward and inverse kinematics were studied and analyzed; finally, the Robotics Toolbox toolbox was used to complete the model creation of X5 underwater manipulator and perform kinematic simulation;

[0058] S110, establish the robot forward motion model,

[0059] Through the given angle of the forward kinematic model of the manipulator, the end of the manipulator can reach the specified position and posture;

[0060] For convenience of representation, sinθ i Abbreviated as s i , cosθ i Abbreviated as c i ,sin(θ 1 +θ 2 ) is abbreviated as s 12 , cos(θ 1 +θ 2 ) is abbreviated as c 12 The Denavit-Hartenberg (DH) parameter modeling method is widely used in underwater manipulator modeling, focusing on clarifying the spatial relationship and motion characteristics between the joints of the manipulator. The improved DH parameter table of the underwater manipulator is shown in Table 1, where the parameter definitions are as follows: joint j, the angle θ between the two common vertical linesj , the distance d between the intersection points of two common perpendicular lines on the same axis j , the distance between the two shafts a j-1 , the angle between the two axes

[0061] Table 1 Parameters of the improved DH underwater manipulator

[0062]

[0063] According to the above DH parameters, the change matrix of each adjacent joint can be obtained; the transformation matrix of each link is defined as follows:

[0064]

[0065] Multiply the transformation matrices of each link in sequence to obtain the transformation matrix of the manipulator end coordinate system relative to the base coordinate system for:

[0066]

[0067] Among them, according to the matrix multiplication rule, we can get:

[0068]

[0069] Link transformation matrix is the function matrix of the six joint variables of the X5 manipulator. By substituting the variables of each joint into this matrix, the position and posture of the manipulator end in the spatial coordinate system can be obtained;

[0070] S120, establish the reverse motion model of the manipulator,

[0071] Common methods for solving inverse kinematics include algebraic method, graphical method, coordinate transformation method, and iterative method. The present invention takes the X5 manipulator model as an example and uses the principle of algebraic method to derive and solve inverse kinematics.

[0072] When the end position of the manipulator is known, the formula can be used to reversely solve the joint variables θ 1 ~θ 6 The value of Multiplying both sides on the left gives Right now:

[0073]

[0074] because It is a symmetric matrix, and using the triangular transformation formula, it can be simplified by calculation:

[0075]

[0076] Since θ in the formula 1 ,θ 3 ± signs appear, so the solution for θ is 1 With θ 3 There are 2 sets of solutions, a total of 4 sets of joint solutions; due to the flipping of each joint, another 4 sets of solutions can be obtained, a total of 8 sets of solutions; due to the range limitations of each joint of the manipulator, some solutions in these 8 sets of solutions cannot be selected. The joint solution closest to the target posture can be selected according to specific requirements;

[0077] S200, using the Monte Carlo method to solve the workspace of the six-degree-of-freedom manipulator, including:

[0078] As a numerical analysis method, the Monte Carlo method estimates probability distribution or solves numerical integration problems in mathematical problems by random sampling. Its basic idea is to represent complex phenomena by random sampling to obtain their statistical regularity. The Monte Carlo method is widely used in estimating integrals that are difficult to solve and solving probability distribution problems. It is also frequently used in robotics. The Monte Carlo method is not limited by degrees of freedom and avoids the problems of complex calculations and poor operability of other numerical methods. The specific solution process is as follows: Figure 7 As shown;

[0079] After initialization, set the number of samples N = 1, define the range of each joint, use the random number generator to randomly generate joint angles, calculate the position of the end effector through forward motion, and determine whether the manipulator is in the workspace. If not, regenerate the joint angles and calculate the end effector position. If so, record the effective position of the manipulator. By setting the number of generated samples N = 30000, the three-dimensional point cloud of the manipulator in the workspace and its projection on each coordinate axis plane are obtained, as shown in Figure 2 As shown;

[0080] S300, based on the 3-5-3 polynomial interpolation method, the trajectory planning of the manipulator is carried out, and the path points in the joint space are obtained by solving the manipulator inverse kinematic model of step S110, and under specific constraints, a function describing the change of the displacement of each joint over time is described; wherein, the first-order derivative of the polynomial interpolation function represents the continuity of velocity, the second-order derivative represents the continuity of acceleration, and the third-order derivative represents the continuity of pulsation, specifically including,

[0081] For each joint variable connecting the initial position and the terminal position, a seventh-order polynomial is sufficient; however, there are many defects in using such a high-order polynomial to interpolate a given node, such as its extreme value is difficult to obtain and it is easy to generate additional motion; therefore, the entire joint space trajectory can be divided into several segments, and different low-order polynomials are used for interpolation in each segment; the 3-5-3 polynomial trajectory used in the present invention is composed of three segments, and the polynomials used in each segment are of different degrees; the first segment is a cubic polynomial, the second segment is a quintic polynomial, and the third segment is a cubic polynomial; compared with the existing 4-3-3 polynomial, the 3-5-3 polynomial used in the present invention has two segments of polynomial trajectory with zero pulsation, which reduces the instability factor; due to the use of a higher-order polynomial, the amount of calculation is reduced;

[0082] The general formula for a 3-5-3 piecewise polynomial is:

[0083]

[0084] Where, j = 1, 2, 3, ..., 6; coefficient a j1i 、a j2i 、a j3i Represents the i-th coefficient of the polynomial interpolation function of the first, second, and third segments of the j-th joint trajectory; θ j1 (t),θ j2 (t),θ j3 (t) represent the first cubic polynomial trajectory, the second quintic polynomial trajectory and the third cubic polynomial trajectory of the joint segment, respectively;

[0085] Assume T j , J j , A j0 、V j , X j They represent the time, pulsation, acceleration, velocity and position of the j-th joint interpolation, respectively. It is known that the initial point X of each segment of the j-th joint j0 、Waypoint X j1 、Waypoint X j2 、Target point X j3 , the acceleration and velocity A of the initial and target points j0 , A j3 、V j0 、V j3 (generally taken as 0), and the velocity and acceleration between each interpolation point are continuous. According to the above conditions, the relationship between the unknown coefficient and the interpolation point can be derived as shown in the following formula:

[0086]

[0087] b=[000000X j3 00 X j000 X j2 X j1 ] T

[0088] a=inv(A)*b

[0089] a=[a j13 a j12 a j11 a j10 a j25 a j24 a j23 a j22 a j21 a j20 a j33 a j32 a j31 a j30 ] T

[0090] Among them, j = 1, 2…6, representing the joints of the underwater manipulator, X j0 ~X j3 It is the path point for the underwater manipulator to run; a j10 ~a j33 is the coefficient of each joint angle equation;

[0091] In the adopted 3-5-3rd order polynomial interpolation method, the interpolation time of each segment is not pre-set, but adjusted according to the specific situation; by exploring the influence of different interpolation times on the underwater manipulator trajectory planning, the influence of each interpolation segment on the underwater manipulator motion parameters (angle, speed, acceleration, jerk) at different interpolation times is studied; by comparing the change curves of related motion parameters at different interpolation times, the optimization path of the underwater manipulator trajectory is explored; Figure 3 The three interpolation times in are 5s each, totaling 15s; Figure 4 The three interpolation times are 3s, 8s, and 4s respectively, and the total simulation time is also 15s. Figure 3 , Figure 4 The quantitative analysis of the data shows that the quantitative analysis table of the effect evaluation under different interpolation times based on the 3-5-3 polynomial interpolation method is shown in Table 2 below.

[0092] Table 2 Quantitative analysis of the effect evaluation based on 3-5-3 polynomial interpolation method at different interpolation times

[0093]

[0094] From the quantitative analysis table of the effect evaluation under different interpolation times based on the 3-5-3 polynomial interpolation method, it can be seen that the maximum acceleration of the interpolation time combination of 3s-8s-4s is 50% smaller than that of the interpolation time combination of 5s-5s-5s, and the maximum jerk is about 46% smaller, and the energy consumption and impact force of the system are greatly reduced; therefore, by changing the interpolation time of the piecewise polynomial, not only the joint smoothness of the manipulator can be changed, but also the energy consumed and the impact suffered by the manipulator can be reduced;

[0095] In summary, the unoptimized polynomial interpolation method causes unnecessary impact on the driving motor of the underwater manipulator and consumes more energy. The key to solving this problem lies in how to select a suitable intelligent optimization algorithm, optimize each interpolation time, and plan the trajectory with the best performance.

[0096] S400, after performing trajectory planning for step S300, perform multi-objective trajectory planning for each joint variable of the manipulator using an improved particle swarm algorithm under kinematic constraints, so that the manipulator has the shortest operation time and the smallest jitter during the operation from the starting point to the target point, so as to achieve trajectory optimization of the time-impact-energy multi-objective problem;

[0097] Polynomial interpolation trajectory planning is difficult to handle with traditional optimization methods due to its high order and non-convex hull properties. In order to overcome the shortcomings of polynomial interpolation, a joint space time-impact-energy optimal 3-5-3 polynomial interpolation trajectory planning algorithm based on improved particle swarm algorithm is proposed according to the kinematic constraints of the manipulator. The algorithm uses the simple structure and easy parameter adjustment characteristics of the particle swarm algorithm to search directly in the optimization target space, avoiding the steps of constructing a particle swarm to calculate the mapping of independent variables and dependent variables, reducing the search dimension, simplifying the calculation, and making each segment of interpolation converge within the kinematic constraints as soon as possible.

[0098] S410, optimization goals are established,

[0099] Time performance indicators:

[0100] Impact performance indicators:

[0101] Energy performance indicators:

[0102] Among them, t i is the running time of the robot; n is the number of path points; a i is the joint acceleration, which is the second-order derivative of the joint trajectory; J i is the impact amount, which is the third-order derivative of the joint trajectory; N is the number of manipulator joints;

[0103] By using the impact performance index S 2 , Energy performance index S3 Considering the corresponding acceleration constraint, the jerk constraint method transforms the time-energy-impact multi-objective problem into a single-objective optimization problem;

[0104] The objective function is:

[0105] f(t)=min(t j1 +t j2 +t j3 )

[0106] stmax{v ji (t)}≤V max ; max{a ji (t)}≤A max ; max{j ji (t)}≤J max

[0107] Among them, t j1 ,t j2 ,t j3 They represent the running time of the three trajectories of the j-th joint of the underwater manipulator respectively;

[0108] S420, constraint items are established,

[0109] For multi-objective trajectory planning in joint space, kinematic parameters are used as constraints, including the displacement, velocity, acceleration and jerk numerical function value of each joint; while considering whether the acceleration and jerk are continuous, the multi-objective comprehensive optimal trajectory is solved for the determined optimization target by satisfying the constraints of each joint; among which, the constraints are as follows:

[0110] Position constraint: |θ ji (t)|≤Θ jmax

[0111] Speed ​​Constraints:

[0112] Acceleration constraints:

[0113] Jerk constraint:

[0114] In the formula, Θ jmax 、V jmax , A jmax , J jmax They represent the joint limit extreme value, joint velocity extreme value, joint acceleration extreme value and joint jerk extreme value respectively;

[0115] S500, using the improved particle swarm algorithm, optimizes the variable trajectory of each joint of the manipulator under kinematic constraints to obtain a multi-objective optimization trajectory that satisfies the optimal time-impact-energy, including:

[0116] In view of the problem that the standard particle swarm algorithm is prone to fall into the local optimum and converges slowly in the later stage, a method of dynamic inertia weight adjustment is proposed. This method is usually adjusted based on the number of iterations or the performance of the particles; in the early stage of iteration, the algorithm tends to let the particles search in a larger range, so a larger inertia weight is set; when the algorithm is executed in the middle and late stages, the particles are close to the area of ​​the global optimal solution. At this time, reducing the inertia weight can help the particles to search more finely in order to find a more accurate optimal solution. Considering various factors comprehensively, the inertia weight should be in the form of exponential decline for the best effect; therefore, the weight adaptive adjustment formula is set as:

[0117]

[0118] Among them, the minimum weight ω min The value is 0.1, the maximum weight ω max The value is 0.9, k is the current iteration number, and N is the maximum iteration number;

[0119] The strategy of dynamically adjusting the learning factor is adopted. In the early stage of the algorithm, the self-learning factor c is 1 Set to be higher than the group learning factor c 2 , giving higher priority to individual cognition to enhance the extensive exploration of the search space; then, in the later stage of iteration, to accelerate the search for the global optimal solution, c 1 is set to be lower than c 2 , in order to promote the coordination of individual cognition and collective behavior, effectively avoiding the algorithm from falling into suboptimal solutions; this dynamic adjustment mechanism of learning factors appropriately reflects the coordination and adaptation between individuals and groups, ensuring the efficiency and robustness of particle swarm optimization algorithm in solving complex problems; the present invention designs a method that combines the iteration cycle to give c 1 and c 2 Assignment, c 1 ,c 2 The constructed function is as follows:

[0120]

[0121] After using the improved particle swarm optimization algorithm, the change curves of inertia weight and learning factor with evolutionary generations are given by Figure 8 It can be seen that the above particle swarm optimization improvement strategy provides appropriate flexibility for the algorithm at different stages in the search space, which helps to better balance the needs of global search and local search.

[0122] Through the above improvements to the standard PSO algorithm, the iterative change curve with each interpolation time as the independent variable and the shortest total time, the lowest energy used, and the smallest impact on the manipulator as the objective function is as follows: Figure 5 As shown; it can be observed that after the improvement, each joint can reach the optimal fitness at an earlier evolutionary generation, and the fitness function is better; from the above experimental data, it can be proved that the improved PSO optimization can obtain the optimal trajectory faster.

[0123] The improved particle swarm algorithm is used to optimize the variable trajectory of each joint of the manipulator under kinematic constraints. The position, velocity and acceleration of each joint after optimization are shown in Figure 2. Figure 6 As shown in Table 3, the optimal interpolation time of each joint after standard particle swarm optimization is shown in Table 3, and the optimal interpolation time of each joint after improved particle swarm optimization is shown in Table 4. After optimization, the manipulator meets the relevant kinematic constraints, and the obtained joint speed and acceleration are continuous. Figure 6 The changes in the positions of the joints shown are relatively stable, indicating that this optimization has played a significant role in shortening the running time. Figure 6 It is observed that the initial and terminal velocities of each joint are both 0, and the velocities are all lower than the maximum operating speed limit. The acceleration at the initial and final positions of each joint is 0, which meets the requirements. From the above experimental data, it can be proved that the polynomial interpolation algorithm after improved PSO optimization has better performance in terms of running time and operation smoothness, and can meet the kinematic constraints.

[0124] Table 3 Optimal interpolation schedule for each joint after standard particle swarm algorithm optimization

[0125]

[0126] Table 4 Optimal interpolation schedule for each joint after improved particle swarm algorithm optimization

[0127]

[0128] From the table above, we can see that the total operation time is 11.348s when the maximum interpolation time of each segment is taken, while the total operation time is 12.277s after the standard PSO algorithm is optimized. After the improved PSO algorithm is optimized, the acceleration and jerk of the manipulator become smaller, and the speed change curve is closer to the maximum speed, which means that the manipulator has higher operation efficiency, smoother joint trajectory, smaller impact, and less energy used in the operation of the manipulator, which meets the time-impact-energy multi-objective optimization requirements set during trajectory optimization design.

[0129] In order to evaluate the advantages and disadvantages of the trajectory optimization method, the optimization effect evaluation is quantitatively analyzed, as shown in Table 5 below.

[0130] Table 5 Quantitative analysis of trajectory optimization method effect evaluation

[0131]

[0132] It can be seen from Table 5 that compared with the basic PSO optimization algorithm, the improved PSO optimization algorithm reduces the total operation time by 0.929s, and the total operation time is reduced by about 7.57%; the energy value used for the manipulator operation is calculated by referring to the energy performance model for each joint angular acceleration value. The energy value used for the improved PSO operation is calculated to be 0.00257 less than that of the standard PSO, and the energy value used is reduced by about 20%; the impact performance model shown by the jerk value of each joint angle is calculated to obtain the impact amount of the manipulator operation. The impact amount of the improved PSO planning trajectory is calculated to be 0.0057 less than that of the standard PSO, and the impact amount is reduced by about 12%; this shows that compared with the standard PSO optimization algorithm, the underwater manipulator after optimization by the improved PSO algorithm reduces the total operation time, reduces the energy consumption, and alleviates the impact on the manipulator.

[0133] Specific implementation scheme two: The present invention provides a system for optimizing the intelligent operation trajectory of an autonomous manipulator for an amphibious unmanned platform. The system has a program module corresponding to the above steps, and executes the steps in the above method for optimizing the intelligent operation trajectory of an autonomous manipulator for an amphibious unmanned platform during operation.

[0134] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0135] Specific implementation scheme three: The present invention provides a computer-readable storage medium, which stores a computer program, and the computer program is configured to implement the steps of a method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform when called by a processor.

[0136] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0137] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.

Claims

1. A method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform, characterized in that: The following steps are involved: S100, establishing a manipulator motion model, including a forward motion model and an inverse motion model, wherein the forward motion model obtains the position and posture of the end effector relative to the base coordinate system through coordinate transformation, and the inverse motion model infers the value of each joint variable when the position and posture of the key points of the end effector path are known; S200, using the Monte Carlo method to solve the workspace of the six-degree-of-freedom manipulator, and obtaining a three-dimensional point cloud image of the manipulator in the workspace and its projection on each coordinate axis plane; S300, performing trajectory planning of the manipulator based on the 3-5-3 polynomial interpolation method, using the manipulator inverse kinematics model of step S100 to solve the path points in the joint space, and describing the function of the displacement of each joint over time under specific constraints; S400, using an improved particle swarm algorithm, introducing dynamic adaptive adjustment of inertia weights and dynamic adjustment of learning factors, optimizes the variable trajectory of each joint of the manipulator under kinematic constraints, and obtains a multi-objective optimization trajectory that meets the optimal time-impact-energy requirements.

2. The method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform according to claim 1 is characterized in that: In step S100, it includes: S110, establishing a forward kinematic model of the manipulator, and enabling the end of the manipulator to reach a specified position and posture through a given angle through the forward kinematic model of the manipulator; Set DH parameters, including joint j, angle θ between two common vertical lines j , the distance d between the intersection points of two common perpendicular lines on the same axis j 、The distance between the two shafts a j-1 , the angle between the two axes And the joint range, the change matrix of each adjacent joint is as follows: Where sinθ i Abbreviated as s i , cosθ i Abbreviated as c i , sin(θ1+θ2) is abbreviated as s 12 , cos(θ1+θ2) is abbreviated as c 12 ; Multiply the transformation matrices of each link in sequence to obtain the transformation matrix of the manipulator end coordinate system relative to the base coordinate system for: Among them, according to the matrix multiplication rule, we can get: Transformation matrix via link Substitute the variables of each joint to obtain the position and posture of the end of the manipulator in the spatial coordinate system.

3. The method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform according to claim 2 is characterized in that: In step S200, after initialization, the number of samples N=1 is set, the range of each joint is defined, the joint angles are randomly generated using a random number generator, the position of the end effector is calculated by forward motion, and it is determined whether the manipulator is in the workspace. If not, the joint angles are regenerated and the end effector position is calculated. If so, the effective position of the manipulator is recorded. By setting the number of generated samples N=30,000, a three-dimensional point cloud map of the manipulator in the workspace and its projection on each coordinate axis plane are obtained.

4. The method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform according to claim 3 is characterized in that: In step S300, it includes: The general formula for a 3-5-3 piecewise polynomial is: Where, j = 1, 2, 3, ..., 6; coefficient a j1i 、a j2i 、a j3i Represents the i-th coefficient of the polynomial interpolation function of the first, second, and third segments of the j-th joint trajectory; θ j1 (t),θ j2 (t),θ j3 (t) represent the first cubic polynomial trajectory, the second quintic polynomial trajectory and the third cubic polynomial trajectory of the joint segment, respectively; Assume T j , J j , A j0 、V j , X j They represent the time, pulsation, acceleration, velocity and position of the j-th joint interpolation respectively. Given the initial point X of each segment of the j-th joint j0 、Waypoint X j1 、Waypoint X j2 、The target point is X j3 , the acceleration and velocity of the initial point and the target point are A j0 , A j3 、V j0 、V j3 , and the velocity and acceleration between each interpolation point are continuous. According to the above conditions, the relationship between the unknown coefficient and the interpolation point can be derived as shown in the following formula: b=[000000X j3 00 X j0 00 X j2 X j1 ] T a=inv(A)*b a=[a j13 a j12 a j11 a j10 a j25 a j24 a j23 a j22 a j21 a j20 a j33 a j32 a j31 a j30 ] T Among them, j = 1, 2…6, representing the joints of the underwater manipulator, X j0 ~X j3 It is the running path point of the underwater manipulator; a j10 ~a j33 are the coefficients of the angle equations of each joint.

5. The method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform according to claim 4 is characterized in that: In step S400, it includes: The weight adaptive adjustment formula is set as: Among them, the minimum weight ω min The value is 0.1, the maximum weight ω max The value is 0.9, k is the current iteration number, and N is the maximum iteration number; The learning factor is dynamically adjusted. In the initial stage of the algorithm, the self-learning factor c1 is set higher than the group learning factor c2. In the later stage of iteration, c1 is set lower than c2. The constructors of c1 and c2 are as follows:

6. An intelligent operation trajectory optimization system for autonomous manipulators of amphibious unmanned platforms, characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 5 above, and executes the steps in the above-mentioned method for optimizing the intelligent operation trajectory of the autonomous manipulator of the amphibious unmanned platform when running.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of the method for optimizing the intelligent operation trajectory of an autonomous manipulator of an amphibious unmanned platform according to any one of claims 1 to 5 when called by a processor.

Images