A picking robot arm trajectory planning method, device, terminal and medium

Abstract

This invention relates to the field of robotic arm trajectory planning technology, specifically disclosing a method, device, terminal, and medium for planning the trajectory of a harvesting robotic arm. The method includes: establishing a three-dimensional scene model of the robotic arm and its operating environment; constructing a collision detection model for the robotic arm during operation to determine a collision-free planned path; using a fifth-order polynomial interpolation function to initially fit the joint trajectory of the robotic arm, determining a preliminary fifth-order polynomial trajectory function of the robotic arm on the planned path; using a cubic B-spline curve to smooth and optimize the preliminary trajectory function of the planned path, constructing a calculated trajectory function for the harvesting robotic arm; using an improved particle swarm optimization algorithm to optimize the calculated trajectory function with the shortest possible movement time of the harvesting robotic arm as the optimization objective, obtaining the time-optimized trajectory function for the harvesting robotic arm. This invention achieves high-precision trajectory planning for harvesting robotic arms in complex agricultural operating environments.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm trajectory planning technology, specifically relating to a method, device, terminal, and medium for planning the trajectory of a harvesting robotic arm. Background Technology

[0002] With the development of modern agriculture, automated and intelligent harvesting technologies have gradually become key means to improve fruit harvesting efficiency and reduce labor costs. Apples, as a major fruit variety in my country, have a concentrated harvesting period and high labor intensity, making them particularly reliant on human resources during the harvesting process. In recent years, harvesting robots, as intelligent equipment integrating visual recognition, trajectory planning, and servo control, have become a hot research area in agricultural engineering.

[0003] In existing technologies, path planning methods for harvesting robotic arms mainly include offline planning methods based on traditional kinematic models and online planning methods that combine environmental perception. Some technologies employ path search algorithms such as A*, RRT (Rapid Expanding Random Tree), and DWA (Dynamic Window Method) to achieve collision-free path planning from the starting point to the target point. However, these methods struggle to balance path smoothness and time optimization, and their convergence speed is slow and prone to getting trapped in local optima in complex environments. To achieve smooth path tracking, some studies have introduced polynomial interpolation or spline fitting methods, but these methods fail to fully consider the dynamic constraints of the robotic arm and the performance limitations of the actuators, making trajectory deployment difficult in practice.

[0004] In addition, for path optimization problems, intelligent optimization algorithms such as particle swarm optimization (PSO) are widely used in robotic arm motion planning. However, the original PSO algorithm has shortcomings in convergence speed, local escape capability, and dynamic adjustment, which limits its adaptability and optimization efficiency in actual harvesting operations. Summary of the Invention

[0005] This invention addresses the problems in the prior art by providing a method, device, terminal, and medium for planning the trajectory of a picking robot arm, which solves the problems of slow convergence speed and easy getting trapped in local optima in the trajectory planning scheme of the robot arm in complex environments.

[0006] The technical solution adopted in this invention is as follows:

[0007] Firstly, this application provides a trajectory planning method for a harvesting robotic arm, comprising the following steps:

[0008] Step S1: Establish a three-dimensional model, which includes a three-dimensional scene model of the robotic arm and its working environment, a model of elements in the scene that can collide, a model of non-picking targets, and a model of picking targets.

[0009] Step S2: Based on the established 3D model, construct a collision detection model for the robotic arm during operation, and determine a collision-free planning path based on the 3D model and the collision detection model.

[0010] Step S3: Based on the joint angles, angular velocities, and angular accelerations of the starting and ending points of the planned path determined in Step S2, a fifth-order polynomial interpolation function is used to perform a preliminary fitting of the joint trajectory of the robotic arm, thereby determining the preliminary fifth-order polynomial trajectory function of the robotic arm on the planned path.

[0011] Step S4: Using a fifth-order polynomial function as the time scale function, and using a cubic B-spline curve to smooth and optimize the preliminary trajectory function of the planned path, the calculation trajectory function of the harvesting robot arm is constructed.

[0012] Step S5: With the goal of minimizing the movement time of the picking robot arm, the improved particle swarm optimization algorithm is used to optimize the trajectory function under the constraints of the robot arm's rated angular velocity, rated angular acceleration, and rated torque, to obtain the optimal trajectory function of the picking robot arm with the best time.

[0013] Preferably, in step S1, the CBB envelope box method is used to construct the robotic arm linkage model and the element model that can collide; the SBB envelope box method is used to construct the non-picking target model.

[0014] Preferably, step S2 includes the following steps:

[0015] Step S2-1: Based on the established 3D model, construct a collision detection model for the robotic arm during operation. The collision detection model includes boundary descriptions of obstacles, motion constraints of the robotic arm, and collision determination rules.

[0016] Step S2-2: Model the motion control process of the robotic arm as a decision model based on state, action, state transition and reward / punishment mechanism;

[0017] Step S2-3: Use the reinforcement learning Informed-Q algorithm to train the decision model of the robotic arm to obtain the path planning model, and determine a collision-free planned path based on the path planning model.

[0018] Preferably, in step S3, the joint angles of the robotic arm are calculated using a fifth-order polynomial interpolation function. Regarding time Functions:

[0019] (1)

[0020] In the formula These are the unknown coefficients of the fifth-order interpolation function;

[0021] Joint angular velocity of the robotic arm and joint angular acceleration The expressions are as follows:

[0022] (2)

[0023] The fifth-order polynomial interpolation trajectory constrains the start and end joint angles, joint angular velocities, and joint angular accelerations; that is, the start and end times of the robotic arm's motion are... and Then the joint angle, joint angular velocity, and joint angular acceleration at the start and end times are respectively:

[0024] (3)

[0025] Substituting equation (3) into equations (1) and (2) yields the initial trajectory function of the robotic arm.

[0026] Preferably, in step S4, the expression for the k-th degree B-spline curve is:

[0027]

[0028] In the formula, Joint angle vector, is a control point, which represents a path point in this application; u is the time scale, and its value range is [0, 1]; Let be the basis function of the B-spline curve, and its expression is:

[0029]

[0030] In the formula, k is the degree of the B-spline and k=3. Let m be a node, and the number of nodes is m+k+1.

[0031] Preferably, let the time scale u be a fifth-degree polynomial with respect to time t, its expression is:

[0032]

[0033] Its first and second derivatives are:

[0034]

[0035]

[0036] Find:

[0037]

[0038] In the formula, Let be a 15th-degree polynomial function, its expression is:

[0039]

[0040] in, The coefficients of the polynomial;

[0041] The unknown coefficients of the trajectory function of the harvesting robotic arm are about The constant, solve Determine the trajectory function, i.e., the start and end times of the harvesting robot arm's movement. and Then the time scale, joint angular velocity, and joint angular acceleration at the start and end times are:

[0042]

[0043] The expression for the joint angular velocity of the harvesting robotic arm is:

[0044] ;

[0045] In the formula, ;

[0046] The expression for the joint angular acceleration of the harvesting robotic arm is:

[0047] ;

[0048] In the formula, ;

[0049] Substituting the angular velocities of the robotic arm joints at the start and end times into the expression for joint angular velocity, and transforming it, we get:

[0050]

[0051] Substituting the joint angular accelerations of the robotic arm at the start and end times into the expression for joint angular acceleration, and transforming it, we get:

[0052]

[0053] Calculate the unknown coefficients The expression is:

[0054]

[0055] The expressions for calculating the joint angles, joint angular velocities, and joint angular accelerations of the harvesting robot arm are the trajectory calculation functions.

[0056] Preferably, in step S5, a particle swarm containing N particles is defined, with the particles randomly distributed in the solution space. The movement speed and position of each particle are iteratively adjusted, and the iterative formula is as follows:

[0057]

[0058] In the formula, Inertial weight; , Individual learning factors and group learning factors; Let i be the position of the i-th particle; Let be the velocity of the i-th particle; Let be the optimal position of the i-th particle; The optimal position for all particles; , It can be any value between [-1, 1].

[0059] By dynamically adjusting the inertia weights and learning factors, an improved PSO algorithm is obtained. The improved inertia weights... Learning factor , The dynamic adjustment formula is:

[0060]

[0061] In the formula, and These are the maximum and minimum values ​​of the inertia weight, respectively. and These are the initial and final values ​​of the individual learning factor; and These are the initial and final values ​​of the group learning factor; This represents the current iteration number; This represents the maximum number of iterations.

[0062] This application uses time as the optimization objective and employs an improved PSO algorithm to find the optimal trajectory function that minimizes the movement time of the picking robot arm. Considering the trajectory optimization of the first three joints, the rated angular velocity, rated angular acceleration, and rated torque of the motor are as follows:

[0063] .

[0064] Secondly, this application provides a trajectory planning device for a harvesting robotic arm, comprising:

[0065] The 3D modeling module is used to collect data about the target environment and build a 3D model of the target environment.

[0066] The collision detection and path planning module is used to detect obstacles in the 3D model and plan a collision avoidance path from the initial position of the robotic arm to the target position based on the detection results and the kinematic model of the robotic arm.

[0067] The trajectory fitting module is used to perform curve fitting or interpolation on the collision avoidance path to generate a continuous and smooth motion trajectory.

[0068] The trajectory optimization module is used to optimize the motion trajectory while satisfying the kinematic and dynamic constraints of the robotic arm.

[0069] The time optimization module, based on the improved particle swarm optimization algorithm, is used to optimize the time parameters of the optimized motion trajectory using the improved particle swarm optimization algorithm, so as to minimize the total time for the robotic arm to complete the trajectory.

[0070] Thirdly, this application provides a terminal, including:

[0071] The memory is used to store the trajectory planning program of the harvesting robot arm;

[0072] A processor is configured to implement the steps of the harvesting robot trajectory planning method as described in the first aspect when executing the trajectory planning action of the harvesting robot arm.

[0073] Fourthly, this application provides a computer-readable storage medium that stores computer instructions. When a computer reads the computer instructions from the storage medium, the computer executes a trajectory planning method for a picking robotic arm as described in the first aspect.

[0074] As can be seen from the above technical solutions, this application has the following advantages:

[0075] 1. This invention establishes a three-dimensional work scene model and combines techniques such as collision detection, fifth-order polynomial trajectory fitting, B-spline smoothing optimization, and an improved particle swarm optimization algorithm to achieve high-precision trajectory planning for a harvesting robotic arm in complex agricultural work environments. The planned path is collision-free, has smooth trajectory, and optimal execution time, effectively improving the robotic arm's motion stability and work efficiency, and significantly reducing energy consumption and the burden on the control system.

[0076] 2. By introducing the CBB envelope box method to establish the robot arm linkage and collision element model, and the SBB method to construct the non-picking target model, the computational complexity can be simplified while ensuring modeling accuracy, and the real-time performance and accuracy of collision detection can be improved, thereby improving the response efficiency and stability of the overall path planning system.

[0077] 3. The path planning problem of the robotic arm is modeled as a reinforcement learning problem based on the state-action-reward mechanism and trained using the Informed-Q algorithm. This can fully explore the prior information in the working environment, improve the path search efficiency and global optimization capability, and make the obtained path safer and more flexible. It is particularly suitable for agricultural scenarios with dynamic obstacle distribution or uncertain targets.

[0078] 4. By using a fifth-order polynomial interpolation function to fit the joint angle trajectory, the continuity of the trajectory can be guaranteed while strictly meeting the angle, speed and acceleration constraints of the starting and ending positions of the picking robot arm. This makes the trajectory more consistent with the dynamic characteristics of the robot arm control system, which helps to reduce joint vibration and position deviation and improve execution accuracy.

[0079] 5. By further smoothing and optimizing the fifth-order polynomial fitted trajectory using cubic B-spline curves, the continuity and differentiability of the trajectory are effectively improved while ensuring path accuracy, reducing abrupt changes in velocity and acceleration, thereby enhancing the stability of the harvesting robot during execution and extending the equipment's lifespan. By setting the time scale to a fifth-order polynomial function and deriving the trajectory velocity and acceleration expressions using a fifteenth-order composite function, unified trajectory control under time parameterization can be achieved, facilitating precise scheduling and optimization within the time domain, and further enhancing the flexibility and accuracy of trajectory control.

[0080] 6. By introducing an improved particle swarm optimization algorithm and dynamically adjusting the inertia weight and learning factor, the convergence speed and ability to escape local optima during the search process can be effectively improved. Optimization is performed with the shortest possible movement time of the robotic arm as the objective function, so that the final trajectory takes into account both time efficiency and mechanical constraints, significantly improving the response speed and energy utilization of the operating system. Attached Figure Description

[0081] To more clearly illustrate the technical solution of this application, the accompanying drawings used in the description will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0082] Figure 1 Here is a flowchart of a trajectory planning method for a harvesting robotic arm as shown in some embodiments;

[0083] Figure 2 Here are some examples of fifth-order polynomial interpolation trajectory plots;

[0084] Figure 3 Here are some examples of seventh-order polynomial interpolation trajectory plots;

[0085] Figure 4 Here are some path planning comparison diagrams shown in the embodiments;

[0086] Figure 5 These are effect diagrams of the calculated trajectory curves shown in some embodiments;

[0087] Figure 6 The following are fitness curves of the Rastrigin function as shown in some embodiments. Detailed Implementation

[0088] To make the purpose, features, and advantages of this application more apparent and understandable, specific embodiments and accompanying drawings will be used to clearly and completely describe the technical solution protected by this application. Obviously, the embodiments described below are only some embodiments of this application, and not all embodiments. Based on the embodiments in this patent, all other embodiments obtained by those skilled in the art without inventive effort are within the scope of protection of this patent.

[0089] With the development of modern agriculture, automated and intelligent harvesting technologies have gradually become key means to improve fruit harvesting efficiency and reduce labor costs. Apples, as a major fruit variety in my country, have a concentrated harvesting period and high labor intensity, making them particularly reliant on human resources during the harvesting process. In recent years, harvesting robots, as intelligent equipment integrating visual recognition, trajectory planning, and servo control, have become a hot research area in agricultural engineering.

[0090] In existing technologies, path planning methods for harvesting robotic arms mainly include offline planning methods based on traditional kinematic models and online planning methods that combine environmental perception. Some technologies employ path search algorithms such as A*, RRT (Rapid Expanding Random Tree), and DWA (Dynamic Window Method) to achieve collision-free path planning from the starting point to the target point. However, these methods struggle to balance path smoothness and time optimization, and their convergence speed is slow and prone to getting trapped in local optima in complex environments. To achieve smooth path tracking, some studies have introduced polynomial interpolation or spline fitting methods, but these methods fail to fully consider the dynamic constraints of the robotic arm and the performance limitations of the actuators, making trajectory deployment difficult in practice.

[0091] In addition, for path optimization problems, intelligent optimization algorithms such as particle swarm optimization (PSO) are widely used in robotic arm motion planning. However, the original PSO algorithm has shortcomings in convergence speed, local escape capability, and dynamic adjustment, which limits its adaptability and optimization efficiency in actual harvesting operations.

[0092] This invention addresses the problems in the prior art by providing a method, device, terminal, and medium for planning the trajectory of a picking robot arm, which solves the problems of slow convergence speed and easy getting trapped in local optima in the trajectory planning scheme of the robot arm in complex environments.

[0093] like Figure 1 As shown, this application provides a trajectory planning method for a harvesting robotic arm, including the following steps:

[0094] Step S1: Establish a three-dimensional model, which includes a three-dimensional scene model of the robotic arm and its working environment, a model of elements in the scene that can collide, a model of non-picking targets, and a model of picking targets.

[0095] Step S2: Based on the established 3D model, construct a collision detection model for the robotic arm during operation, and determine a collision-free planning path based on the 3D model and the collision detection model.

[0096] Step S3: Based on the joint angles, angular velocities, and angular accelerations of the starting and ending points of the planned path determined in Step S2, a fifth-order polynomial interpolation function is used to perform a preliminary fitting of the joint trajectory of the robotic arm, thereby determining the preliminary fifth-order polynomial trajectory function of the robotic arm on the planned path.

[0097] Step S4: Using a fifth-order polynomial function as the time scale function, and using a cubic B-spline curve to smooth and optimize the preliminary trajectory function of the planned path, the calculation trajectory function of the harvesting robot arm is constructed.

[0098] Step S5: With the goal of minimizing the movement time of the picking robot arm, the improved particle swarm optimization algorithm is used to optimize the trajectory function under the constraints of the robot arm's rated angular velocity, rated angular acceleration, and rated torque, to obtain the optimal trajectory function of the picking robot arm with the best time.

[0099] In some embodiments, the geometric model of the robotic arm and fruit tree branches is constructed using the CBB envelope box method. Based on the actual situation of the robotic arm in the harvesting scenario, link 1 will not collide with obstacles, therefore only the models of links 2-6 need to be constructed. Furthermore, the relative position of link 6 does not change significantly during the robotic arm's movement; therefore, a cylinder is used to represent link 6, further simplifying the model.

[0100] To facilitate mechanized fruit harvesting, the fruit trees are grown using a trellis system; the SBB envelope box method is used to construct the geometric model of the non-target fruits.

[0101] In some embodiments, based on the model of the robotic arm and obstacles constructed above, the collision detection situation is divided into two types: collision detection between the robotic arm and the fruit tree branches and collision detection between the robotic arm and non-target fruits, which can be equivalent to collision detection between cylinders and cylinders and collision detection between cylinders and spheres.

[0102] , , ... This represents the distance between AB and CD. This represents the distance between AB and O. When At that time, the robotic arm linkage and the fruit tree branch did not collide; when At that time, the robotic arm linkage did not collide with the non-target fruit.

[0103] In some embodiments, a robotic arm collision detection algorithm is used to obtain the robotic arm's... Since the six joints of the robotic arm have little impact on the position of the robotic arm linkage model, collision detection is mainly performed on the first three joints of the robotic arm.

[0104] In some embodiments, step S2 includes the following steps:

[0105] Step S2-1: Based on the established 3D model, construct a collision detection model for the robotic arm during operation. The collision detection model includes boundary descriptions of obstacles, motion constraints of the robotic arm, and collision determination rules.

[0106] Step S2-2: Model the motion control process of the robotic arm as a decision model based on state, action, state transition and reward / punishment mechanism;

[0107] Step S2-3: Use the reinforcement learning Informed-Q algorithm to train the decision model of the robotic arm to obtain the path planning model, and determine a collision-free planned path based on the path planning model.

[0108] In some embodiments, the robot arm joint angles are calculated using a fifth-order polynomial interpolation function. Regarding time Functions:

[0109] (1)

[0110] In the formula These are the unknown coefficients of the fifth-order interpolation function.

[0111] Then the joint angular velocity of the robotic arm and joint angular acceleration The expressions are as follows:

[0112] (2)

[0113] The fifth-order polynomial interpolation trajectory constrains the start and end joint angles, joint angular velocities, and joint angular accelerations; that is, the start and end times of the robotic arm's motion are... and Then the joint angle, joint angular velocity, and joint angular acceleration at the start and end times are respectively:

[0114] (3)

[0115] Substituting equation (3) into equations (1) and (2) yields the trajectory function of the robotic arm. Solving for the unknown coefficients in Matlab yields the fifth-order polynomial interpolation trajectory, such as... Figure 6 As shown.

[0116] As can be seen from the figure, the angular velocity curve has a smooth profile without inflection points, and the angular acceleration curve is continuous, which avoids joint vibration caused by sudden changes in angular acceleration, enabling the robotic arm to smoothly pass through each path point.

[0117] However, the joint angle curves of the robotic arm deviate from the original path. Although the joint acceleration curves are continuous, their contours are not smooth enough, and the numerical variations are large. Large-scale acceleration and deceleration of the joint motors within a short period will result in excessive inertial forces, causing vibrations during robotic arm operation and reducing its lifespan. Therefore, higher-order polynomial interpolation functions are needed to represent the joint trajectories. Higher-order functions produce smoother joint acceleration curves, which is beneficial for stable robotic arm movement.

[0118] Robotic arm joint angles using seventh-order polynomial interpolation functions Regarding time Functions:

[0119] (4)

[0120] In the formula These are the unknown coefficients of the seventh-order interpolation function.

[0121] As shown in equation (4), the seventh-order interpolation function contains eight unknown coefficients, requiring eight constraints to solve. Therefore, in addition to the joint angles, angular velocities, and angular accelerations of the robotic arm, constraints must also be applied to its angular jerk. Regarding the joint angles of the robotic arm... Joint angular velocity Joint angular acceleration and joint angular acceleration The expressions are as follows:

[0122] (5)

[0123] Similarly, the start and end times of the robotic arm's movement and Then the joint angle, joint angular velocity, joint angular acceleration, and joint angular jerk at the start and end times are respectively:

[0124] (6)

[0125] Substituting equation (6) into equations (4) and (5) yields the seventh-order polynomial trajectory function of the robotic arm.

[0126] In Matlab, the unknown coefficients are solved to obtain the seventh-order polynomial interpolation trajectory, such as... Figure 6 As shown.

[0127] As shown in the figure, compared with the fifth-order polynomial interpolation trajectory, the seventh-order polynomial interpolation trajectory results in a less significant deviation of the robotic arm's joint angle curves from the original path, and the joint velocity and acceleration curves are smoother and without inflection points. The range of joint acceleration variation is also smaller compared to the fifth-order polynomial interpolation trajectory, thus mitigating the problem of excessive joint acceleration variation in the fifth-order polynomial trajectory, which caused robotic arm vibration and severe deviation from the original path.

[0128] Therefore, seventh-order polynomial interpolation trajectories are more suitable for robotic arm trajectory planning.

[0129] The higher the order of the interpolation function for the robotic arm's joint trajectory, the better the planned trajectory performance. However, the coefficients of the polynomial interpolation function also increase with the order, requiring more constraints to solve for the unknown coefficients, thus increasing the difficulty of the solution.

[0130] To avoid collisions between the harvesting robot arm and obstacles, obtain a smooth trajectory function, and reduce the difficulty of solving the problem, this application uses a cubic B-spline function to optimize the path and uses a fifth-order polynomial interpolation function as the expression for the time scale in the cubic B-spline function, thereby obtaining the trajectory of each joint of the harvesting robot arm.

[0131] To address the issue of non-smooth paths generated by the Q-learning algorithm, this application employs B-spline curves to optimize the paths. B-spline curves are a common representation of free-form curves in computer graphics, and are polynomial curves that approximate their control points. The expression for a k-th degree B-spline curve is:

[0132] (7)

[0133] In the formula, Joint angle vector, is a control point, which represents a path point in this application; u is the time scale, and its value range is [0, 1]; Let be the basis function of the B-spline curve, and its expression is:

[0134] (8)

[0135] In the formula, k is the degree of the B-spline. Let m be the number of nodes, and k+1 nodes in total. The fitting effect of the B-spline curve is as follows: Figure 6 As shown in the figure, the cubic B-spline curve is closer to the path obtained by path planning than the quartic and quintic B-spline curves. Therefore, this application uses the cubic B-spline curve to fit the path.

[0136] To avoid impacting the joint motors, the robotic arm trajectory should be a sufficiently smooth function of time and must comply with constraints such as joint velocity, acceleration, or torque. Cubic B-spline curves cannot meet these requirements.

[0137] Let the time scale u be a fifth-degree polynomial with respect to time t, its expression is:

[0138] (9)

[0139] Its first and second derivatives are:

[0140] (10)

[0141] (11)

[0142] Substituting equation (9) into equation (7), we get:

[0143] (12)

[0144] In the formula, Let be a 15th-degree polynomial function, its expression is:

[0145] (13)

[0146] in, are the coefficients of the polynomial.

[0147] Observing equations (12) and (13), it can be seen that the unknown coefficients of the trajectory function of the picking robot arm are about Since the constant is , we only need to find the solution. Its trajectory function can then be determined. Referencing the method for solving the coefficients of a fifth-degree polynomial, this involves determining the start and end times of the robotic arm's movement. and Then the time scale, joint angular velocity, and joint angular acceleration at the start and end times are:

[0148] (14)

[0149] Differentiating equation (12) with respect to time t, we obtain the expression for the angular velocity of the joints of the harvesting robotic arm as follows:

[0150] (15)

[0151] In the formula, Its expression is:

[0152] (16)

[0153] Similarly, taking the second derivative of equation (12) with respect to time t, we obtain the expression for the joint angular acceleration of the harvesting robot arm as follows:

[0154] (17)

[0155] In the formula, Its expression is:

[0156] (18)

[0157] Substituting the angular velocities of the picking robot joints at the start and end times into equation (15) and transforming it, we get:

[0158] (19)

[0159] Substituting the joint angular acceleration of the picking robot arm at the start and end times into equation (17) and transforming it, we get:

[0160] (20)

[0161] Substituting equations (14), (19), and (20) into equations (9), (10), and (11), we get:

[0162] (twenty one)

[0163] Solving equation (21) yields the unknown coefficients. The expression is:

[0164] (twenty two)

[0165] Substituting equation (22) into equations (9), (10) and (11), we obtain the time scale and its first and second derivatives with respect to time. Substituting these into equations (12), (15) and (17), we obtain the expressions for the joint angles, joint angular velocities and joint angular accelerations of the picking robot arm.

[0166] To visually demonstrate the effect of the fifteenth-order polynomial trajectory obtained based on B-splines and polynomial functions in this application, simulations were performed on a two-dimensional map to compare the fifteenth-order polynomial trajectory and the seventh-order polynomial interpolation trajectory. The simulation results are as follows: Figure 6 As shown.

[0167] As can be seen from the figure, the fifteenth-order polynomial trajectory is smoother and closer to the original path than the seventh-order polynomial interpolation trajectory. This is because this application fits the path using a cubic B-spline function, allowing each joint to cooperate with each other, resulting in a smaller deviation of the actual path from the original path. In contrast, the seventh-order polynomial interpolation method interpolates each joint independently, making it impossible to determine the actual path between two interpolation points, thus resulting in a larger deviation from the original path.

[0168] This application will utilize a trajectory planning method based on B-splines and polynomial functions, and a seventh-order polynomial interpolation method, respectively, to plan the trajectory of the simulation path obtained in Section 3.4. Table 1 shows the simulation path points of the harvesting robot arm, and the obtained trajectory planning results of the harvesting robot arm are as follows: Figure 6 As shown.

[0169] Table 1

[0170]

[0171] To improve the efficiency of the harvesting robot arm, it should operate at maximum power within its rated angular velocity, rated angular acceleration, and rated torque range. Therefore, this application uses an optimization algorithm to search for the trajectory of the harvesting robot arm with the shortest movement time within the constraints.

[0172] Particle Swarm Optimization (PSO) is a heuristic optimization algorithm that simulates the foraging behavior of birds. Due to its simple structure and high efficiency and stability, PSO has been widely applied to various optimization problems. In PSO, a particle swarm of N particles is first defined, with the particles randomly distributed in the solution space. Each particle has two attributes: position and velocity, as well as a fitness value determined by the objective function. The fitness value reflects the quality of the particle's position. Based on the current optimal position of the individual particle and the optimal position of the swarm, the movement velocity and position of each particle are iteratively adjusted. The iterative formula is shown below:

[0173] (twenty three)

[0174] In the formula, Inertial weight; , Individual learning factors and group learning factors; Let i be the position of the i-th particle; Let be the velocity of the i-th particle; Let be the optimal position of the i-th particle; The optimal position for all particles; , It can be any value between [-1, 1].

[0175] However, due to the inertia weights of the traditional PSO algorithm and learning factors , The constant value can easily lead to getting trapped in local optima and a trade-off between convergence speed and convergence accuracy. Therefore, this application improves the inertia weight by dynamically adjusting the inertia weight and learning factor. Learning factor , The dynamic adjustment formula is:

[0176] (twenty four)

[0177] In the formula, and These are the maximum and minimum values ​​of the inertia weight, respectively. and These are the initial and final values ​​of the individual learning factor; and These are the initial and final values ​​of the group learning factor; This represents the current iteration number; This represents the maximum number of iterations.

[0178] As can be seen from equation (28), with the increase of the number of iterations, and Gradually decrease As the number of cells increases, the algorithm's global search capability weakens while its local search capability strengthens. Therefore, improving the PSO algorithm can achieve a balance between global and local search capabilities, thus improving the algorithm's overall optimization performance.

[0179] To verify the superiority of the improved PSO algorithm over the traditional PSO algorithm, this application uses the Rastrigin function as the objective function to find the minimum value of the Rastrigin function. The expression of the Rastrigin function is:

[0180] (25)

[0181] When x=0 and y=0, the Rastrigin function has an optimal fitness value. The iterative fitness curves of the two particle swarm optimization algorithms are as follows: Figure 6 As shown:

[0182] The improved PSO algorithm converges significantly faster than the traditional PSO algorithm. The improved PSO algorithm is close to the optimal position by the 25th iteration, while the traditional PSO algorithm only converges to the optimal value by the 47th iteration.

[0183] This application uses time as the optimization objective and employs an improved PSO algorithm to find the trajectory function with the shortest movement time for the picking robot arm. Since the path function is mainly determined by the first three joints, this application only considers the trajectory optimization of the first three joints. Based on the structure of the picking robot arm in this application, the motors of its first three joint modules are of the same model, and the rated angular velocity, rated angular acceleration, and rated torque of the motors are as follows:

[0184] (26).

[0185] In some embodiments, this application provides a trajectory planning device for a harvesting robotic arm, comprising:

[0186] The 3D modeling module is used to collect data about the target environment and build a 3D model of the target environment.

[0187] The collision detection and path planning module is used to detect obstacles in the 3D model and plan a collision avoidance path from the initial position of the robotic arm to the target position based on the detection results and the kinematic model of the robotic arm.

[0188] The trajectory fitting module is used to perform curve fitting or interpolation on the collision avoidance path to generate a continuous and smooth motion trajectory.

[0189] The trajectory optimization module is used to optimize the motion trajectory while satisfying the kinematic and dynamic constraints of the robotic arm.

[0190] The time optimization module, based on the improved particle swarm optimization algorithm, is used to optimize the time parameters of the optimized motion trajectory using the improved particle swarm optimization algorithm, so as to minimize the total time for the robotic arm to complete the trajectory.

[0191] In some embodiments, this application provides a terminal, including:

[0192] The memory is used to store the trajectory planning program of the harvesting robot arm;

[0193] A processor is configured to implement the steps of the harvesting robot trajectory planning method as described in the first aspect when executing the trajectory planning action of the harvesting robot arm.

[0194] In some embodiments, this application provides a computer-readable storage medium that stores computer instructions. When a computer reads the computer instructions in the storage medium, the computer executes the aforementioned method for planning the trajectory of a picking robotic arm.

[0195] The basic concepts have been described above. It is clear that the detailed disclosure above is merely illustrative and does not constitute a limitation of this specification. Although not explicitly stated herein, various modifications, improvements, and corrections may be made to this specification by those skilled in the art. Such modifications, improvements, and corrections are suggested in this specification and therefore remain within the spirit and scope of the exemplary embodiments described herein.

[0196] Furthermore, unless expressly stated in the claims, the order of processing elements and sequences, the use of numbers and letters, or other names described in this specification are not intended to limit the order of the processes and methods described herein. Although various examples have been discussed in the foregoing disclosure of some embodiments of the invention that are currently considered useful, it should be understood that such details are for illustrative purposes only, and the appended claims are not limited to the disclosed embodiments; rather, the claims are intended to cover all modifications and equivalent combinations that conform to the spirit and scope of the embodiments described herein. For example, while the system components described above can be implemented using hardware devices, they can also be implemented solely using software solutions, such as installing the described system on existing servers or mobile devices.

[0197] Similarly, it should be noted that, in order to simplify the description disclosed herein and thus aid in the understanding of one or more embodiments of the invention, the foregoing description of embodiments in this specification may sometimes combine multiple features into a single embodiment, drawing, or description thereof. However, this method of disclosure does not imply that the subject matter of this specification requires more features than those mentioned in the claims. In fact, the embodiments contain fewer features than all the features of a single embodiment disclosed above.

[0198] In some embodiments, numbers describing the quantity of components and attributes are used. It should be understood that such numbers used in the description of embodiments are modified in some examples with the terms "approximately," "approximately," or "generally." Unless otherwise stated, "approximately," "approximately," or "generally" indicates that the numbers are allowed to vary by ±20%. Accordingly, in some embodiments, the numerical parameters used in the specification and claims are approximate values, which may be changed depending on the characteristics required by individual embodiments. In some embodiments, numerical parameters should take into account specified significant digits and employ a general method of digit reservation. Although the numerical ranges and parameters used to confirm their breadth of range in some embodiments of this specification are approximate values, in specific embodiments, such values ​​are set as precisely as feasible.

[0199] Finally, it should be understood that the embodiments described in this specification are merely illustrative of the principles of the embodiments described herein. Other variations may also fall within the scope of this specification. Therefore, alternative configurations of the embodiments described herein are intended to be illustrative rather than limiting, and should be considered consistent with the teachings of this specification. Accordingly, the embodiments described herein are not limited to those explicitly introduced and described herein.

Claims

1. A trajectory planning method for a harvesting robotic arm, characterized in that, Includes the following steps: Step S1: Establish a three-dimensional model, which includes a three-dimensional scene model of the robotic arm and its working environment, a model of elements in the scene that can collide, a model of non-picking targets, and a model of picking targets. Step S2: Based on the established 3D model, construct a collision detection model for the robotic arm during operation, and determine a collision-free planning path based on the 3D model and the collision detection model. Step S2-1: Based on the established 3D model, construct a collision detection model for the robotic arm during operation. The collision detection model includes boundary descriptions of obstacles, motion constraints of the robotic arm, and collision determination rules. Step S2-2: Model the motion control process of the robotic arm as a decision model based on state, action, state transition and reward / punishment mechanism; Step S2-3: Use the reinforcement learning Informed-Q algorithm to train the decision model of the robotic arm to obtain the path planning model, and determine a collision-free planned path based on the path planning model; Step S3: Based on the joint angles, angular velocities, and angular accelerations of the starting and ending points of the planned path determined in Step S2, a fifth-order polynomial interpolation function is used to perform a preliminary fitting of the joint trajectory of the robotic arm, thereby determining the preliminary fifth-order polynomial trajectory function of the robotic arm on the planned path. Step S4: Using a fifth-order polynomial function as the time scale function, and using a cubic B-spline curve to smooth and optimize the preliminary trajectory function of the planned path, the calculation trajectory function of the harvesting robot arm is constructed. Step S5: With the goal of minimizing the movement time of the picking robot arm, the improved particle swarm optimization algorithm is used to optimize the trajectory function under the constraints of the robot arm's rated angular velocity, rated angular acceleration, and rated torque, to obtain the optimal trajectory function of the picking robot arm with the best time. Define a particle swarm containing N particles, with the particles randomly distributed in the solution space. Iteratively adjust the movement speed and position of each particle, using the following iterative formula: In the formula, Inertial weight; , Individual learning factors and group learning factors; Let i be the position of the i-th particle; Let be the velocity of the i-th particle; Let be the optimal position of the i-th particle; The optimal position for all particles; , It can be any value between [-1, 1]. By dynamically adjusting the inertia weights and learning factors, an improved PSO algorithm is obtained. The improved inertia weights... Learning factor , The dynamic adjustment formula is: In the formula, and These are the maximum and minimum values ​​of the inertia weight, respectively. and These are the initial and final values ​​of the individual learning factor; and These are the initial and final values ​​of the group learning factor; This represents the current iteration number; This represents the maximum number of iterations. With time as the optimization objective, an improved PSO algorithm is used to find the optimal trajectory function that minimizes the movement time of the picking robot arm. Considering the trajectory optimization of the first three joints, the rated angular velocity, rated angular acceleration, and rated torque of the motor are as follows: 。 2. The trajectory planning method for a harvesting robotic arm according to claim 1, characterized in that, In step S1, the CBB envelope box method is used to construct the robotic arm linkage model and the element model that can collide; the SBB envelope box method is used to construct the non-picking target model.

3. The trajectory planning method for a harvesting robotic arm according to claim 1, characterized in that, In step S3, the joint angles of the robotic arm are calculated using a fifth-order polynomial interpolation function. Regarding time Functions: （1） In the formula These are the unknown coefficients of the fifth-order interpolation function; Joint angular velocity of the robotic arm and joint angular acceleration The expressions are as follows: （2） The fifth-order polynomial interpolation trajectory constrains the start and end joint angles, joint angular velocities, and joint angular accelerations; that is, the start and end times of the robotic arm's motion are... and Then the joint angle, joint angular velocity, and joint angular acceleration at the start and end times are respectively: （3） Substituting equation (3) into equations (1) and (2) yields the initial trajectory function of the robotic arm.

4. The trajectory planning method for a harvesting robotic arm according to claim 3, characterized in that, In step S4, the expression for the k-th degree B-spline curve is: In the formula, Joint angle vector, In this application, control points are represented as path points; u is the time scale, and its value ranges from [0, 1]. Let be the basis function of the B-spline curve, and its expression is: In the formula, k is the degree of the B-spline and k=3. Let m be a node, and the number of nodes is m+k+1.

5. The trajectory planning method for a harvesting robotic arm according to claim 4, characterized in that, Let the time scale u be a fifth-degree polynomial with respect to time t, its expression is: Its first and second derivatives are: Find: In the formula, Let be a 15th-degree polynomial function, its expression is: in, The coefficients of the polynomial; The unknown coefficients of the trajectory function of the harvesting robotic arm are about The constant, solve Determine the trajectory function, i.e., the start and end times of the harvesting robot arm's movement. and Then the time scale, joint angular velocity, and joint angular acceleration at the start and end times are: The expression for the joint angular velocity of the harvesting robotic arm is: ； In the formula, ; The expression for the joint angular acceleration of the harvesting robotic arm is: ； In the formula, ; Substituting the angular velocities of the robotic arm joints at the start and end times into the expression for joint angular velocity, and transforming it, we get: Substituting the joint angular accelerations of the robotic arm at the start and end times into the expression for joint angular acceleration, and transforming it, we get: Calculate the unknown coefficients The expression is: The expressions for calculating the joint angles, joint angular velocities, and joint angular accelerations of the harvesting robot arm are the trajectory calculation functions.

6. A terminal, characterized in that, include: The memory is used to store the trajectory planning program of the harvesting robot arm; A processor is configured to implement the steps of the harvesting robot trajectory planning method as described in any one of claims 1-5 when performing the trajectory planning action of the harvesting robot.

7. A computer-readable storage medium, characterized in that, The storage medium stores computer instructions. When the computer reads the computer instructions in the storage medium, the computer executes a trajectory planning method for a picking robotic arm as described in any one of claims 1 to 5.

Images