Picking mechanical arm trajectory planning method and device, terminal and medium

Abstract

The invention relates to the technical field of mechanical arm trajectory planning, and particularly discloses a picking mechanical arm trajectory planning method and device, a terminal and a medium. The method comprises the steps that a three-dimensional scene model of the mechanical arm and the working environment of the mechanical arm is established; a collision detection model of the mechanical arm in the operation process is constructed, and a collision-free planned path is determined; adopting a quintic polynomial interpolation function to perform preliminary fitting on a joint track of the mechanical arm, and determining a quintic polynomial preliminary track function of the mechanical arm on the planned path; performing smooth optimization on the preliminary trajectory function of the planned path by using a cubic B-spline curve, and constructing a calculation trajectory function of the picking mechanical arm; and by taking the shortest movement time of the picking mechanical arm as an optimization target, optimizing the calculated trajectory function by using an improved particle swarm algorithm to obtain a picking mechanical arm optimized trajectory function with optimal time. According to the invention, high-precision trajectory planning of the picking mechanical arm in a complex agricultural operation environment is realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotic arm trajectory planning, and particularly relates to a method, device, terminal, and medium for trajectory planning of a picking robotic arm. Background Art

[0002] With the development of modern agriculture, automated and intelligent picking technologies have gradually become the key means to improve fruit picking efficiency and reduce labor costs. As a major fruit variety in China, apples have a concentrated picking period and a high labor intensity, and their dependence on human resources in the picking process is particularly significant. In recent years, picking robots, as intelligent equipment integrating visual recognition, trajectory planning, and servo control, have become a hot research direction in agricultural engineering.

[0003] In the prior art, the path planning methods of picking robotic arms mainly include offline planning methods based on traditional kinematic models and online planning methods combined with environmental perception. Some techniques use path search algorithms such as A*, RRT (Rapidly-exploring Random Trees), and DWA (Dynamic Window Approach) to achieve collision-free path planning from the starting point to the target point. However, such methods are difficult to balance the smoothness and time optimality of the path, and have a slow convergence speed and are prone to falling into local optima in complex environments. To achieve smooth path tracking, some studies introduce polynomial interpolation or spline fitting methods, but they do not fully consider the dynamic constraints of the robotic arm and the performance limitations of the actuators, resulting in trajectories that are difficult to be actually deployed.

[0004] In addition, for the path optimization problem, intelligent optimization algorithms such as Particle Swarm Optimization (PSO) are widely used in robotic arm motion planning. However, the original PSO algorithm has deficiencies in convergence speed, local jumping ability, and dynamic adjustment, resulting in limitations in its adaptability and optimization efficiency in actual picking operations. Summary of the Invention

[0005] In view of the problems in the prior art, the present invention provides a method, device, terminal, and medium for trajectory planning of a picking robotic arm, which solves the problems of slow convergence speed and easy falling into local optima of the robotic arm trajectory planning scheme in complex environments.

[0006] The technical solution adopted by the present invention is as follows: In a first aspect, the present application provides a method for trajectory planning of a picking robotic arm, including 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, an element model that can collide in the scene, a non-picking target model, and a picking target model; Step S2: Based on the established three-dimensional model, construct a collision detection model for the robotic arm during operation, and determine a collision-free planned path based on the three-dimensional model and the collision detection model; Step S3: According to the joint angles, angular velocities, and angular accelerations of the starting and ending points of the planned path determined in Step S2, use a quintic polynomial interpolation function to preliminarily fit the joint trajectory of the robotic arm, and determine the quintic polynomial preliminary trajectory function of the robotic arm on the planned path; Step S4: Use a quintic polynomial function as the time scaling function, and use a cubic B-spline curve to smooth and optimize the preliminary trajectory function of the planned path to construct the calculated trajectory function of the picking robotic arm; Step S5: Taking the shortest movement time of the picking robotic arm as the optimization goal, use an improved particle swarm algorithm to optimize the calculated trajectory function under the constraint conditions of the rated angular velocity, rated angular acceleration, and rated torque of the robotic arm to obtain the time-optimal optimized trajectory function of the picking robotic arm.

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

[0008] Preferably, Step S2 includes the following steps: Step S2-1: Based on the established three-dimensional model, construct a collision detection model during the operation of the robotic arm. The collision detection model includes the boundary description of the obstacle, the motion constraints of the robotic arm, and the collision determination rules; Step S2-2: Model the motion control process of the robotic arm as a decision-making model based on state, action, state transition, and reward and punishment mechanisms; Step S2-3: Use the reinforcement learning Informed-Q algorithm to train the decision-making model of the robotic arm to obtain a path planning model, and determine a collision-free planned path based on the path planning model.

[0009] Preferably, in Step S3, the joint angle of the robotic arm using the quintic polynomial interpolation function is a function of time : (1) where are the unknown coefficients of the quintic interpolation function; The expressions for the joint angular velocity and joint angular acceleration of the robotic arm are respectively: (2) The quintic polynomial interpolation trajectory constrains the starting and ending joint angles, joint angular velocities, and joint angular accelerations, that is, the starting and ending times of the robotic arm movement are and , the joint angle values, joint angular velocity values, and joint angular accelerations at the start and end times are respectively: (3) Substituting Equation (3) into Equations (1) and (2) can obtain the preliminary trajectory function of the robotic arm.

[0010] Preferably, in step S4, the expression of the k-th B-spline curve is:

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

[0012] In the formula, k is the B-spline order and k = 3, are the knots, and the number of knots is m + k + 1.

[0013] Preferably, let the time scale u be a fifth-degree polynomial about time t, and its expression is:

[0014] Its first derivative and second derivative are:

[0015]

[0016] It is obtained that:

[0017] In the formula, is a fifteen-degree polynomial function, and its expression is:

[0018] Among them, are the coefficients of the polynomial; The unknown coefficients of the picking robotic arm trajectory function are constants about , solve to determine the calculated trajectory function, that is, the start and end times of the picking robotic arm movement and , then the time scale, joint angular velocity, and joint angular acceleration at the start and end times are:

[0019] The expression of the picking robotic arm joint angular velocity is: ; In the formula, ; The expression for the joint angular acceleration of the picking robotic arm is: ; In the formula, ; Substitute the joint angular velocities of the picking robotic arm at the start and end times into the expression for the joint angular velocity, and transform to get:

[0020] Substitute the joint angular accelerations of the picking robotic arm at the start and end times into the expression for the joint angular acceleration, and transform to get:

[0021] Calculate the expression for the unknown coefficient as:

[0022] Calculate the expressions for the joint angle, joint angular velocity, and joint angular acceleration of the picking robotic arm, which is to calculate the trajectory function.

[0023] Preferably, in step S5, define a particle swarm containing N particles, make the particles randomly distributed in the solution space, and iteratively adjust the moving speed and position of each particle. The iterative formula is as follows:

[0024] In the formula, is the inertia weight; , are the individual learning factor and the swarm learning factor; is the position of the i-th particle; is the velocity of the i-th particle; is the optimal position of the i-th particle; is the optimal position of all particles; , are arbitrary values between [-1, 1]; Dynamically adjust the inertia weight and learning factor to obtain an improved PSO algorithm. The improved inertia weight , and the learning factors , have the following dynamic adjustment formulas:

[0025] In the formula, and are the maximum and minimum values of the inertia weight respectively; and are the initial value and the final value of the individual learning factor; and are the initial value and the final value of the population learning factor; is the current iteration number; is the maximum iteration number; This application takes time as the optimization objective, uses the improved PSO algorithm to find the optimization trajectory function with the shortest movement time of the picking manipulator, and considers the trajectory optimization of the first three joints. The rated angular velocity, rated angular acceleration, and rated torque of the motor are respectively: .

[0026] In a second aspect, this application provides a picking manipulator trajectory planning device, including: A three-dimensional modeling module, configured to collect data of the target environment and establish a three-dimensional model of the target environment; A collision detection and path planning module, configured to perform obstacle detection in the three-dimensional model, and plan a collision avoidance path from the initial position of the manipulator to the target position according to the detection result and the kinematic model of the manipulator; A trajectory fitting module, which is used to perform curve fitting or interpolation on the collision avoidance path to generate a continuous and smooth motion trajectory; A trajectory optimization module, which is used to optimize the motion trajectory on the premise of satisfying the kinematic and dynamic constraints of the manipulator; A time optimization module, based on the improved particle swarm algorithm, is used to optimize the time parameters of the optimized motion trajectory by using the improved particle swarm algorithm to minimize the total time for the manipulator to complete the trajectory.

[0027] In a third aspect, this application provides a terminal, including: A memory, configured to store a picking manipulator trajectory planning program; A processor, configured to implement the steps of the picking manipulator trajectory planning method as described in the first aspect when performing the picking manipulator trajectory planning action.

[0028] In a fourth aspect, this application provides a computer-readable storage medium. The storage medium stores computer instructions. When a computer reads the computer instructions in the storage medium, the computer executes a picking manipulator trajectory planning method as described in the first aspect.

[0029] It can be seen from the above technical solutions that this application has the following advantages: 1. The present invention realizes high-precision trajectory planning of a picking robotic arm in a complex agricultural operation environment by establishing a three-dimensional operation scenario model and combining technical means such as collision detection, quintic polynomial trajectory fitting, B-spline smoothing optimization, and improved particle swarm optimization algorithm. The planned path has collision-free, trajectory smoothness, and optimal execution time, which can effectively improve the motion stability and operation efficiency of the robotic arm, and significantly reduce energy consumption and the burden on the control system.

[0030] 2. By introducing the CBB envelope box method to establish the robotic arm link and collision element model, and the SBB method to construct the non-picking target model, it is possible to simplify the computational complexity while ensuring the modeling accuracy, improve the real-time performance and accuracy of collision detection, and thus improve the response efficiency and stability of the overall path planning system.

[0031] 3. Modeling the robotic arm path planning problem as a reinforcement learning problem based on a state-action-reward mechanism, and training with the Informed-Q algorithm can fully exploit the prior information in the operation environment, improve the path search efficiency and global optimization ability, making the obtained path more secure and flexible, especially suitable for agricultural scenarios with dynamically distributed obstacles or uncertain targets.

[0032] 4. Using the quintic polynomial interpolation function to fit the joint angle trajectory can ensure the trajectory continuity while strictly satisfying the angle, speed, and acceleration constraints at the start and end positions of the picking robotic arm, so that the trajectory better conforms to the dynamic characteristics of the robotic arm control system, helps reduce joint vibration and position deviation, and improves the execution accuracy.

[0033] 5. Further smoothing and optimizing the quintic polynomial fitting trajectory with a cubic B-spline curve can effectively improve the continuity and differentiability of the trajectory on the basis of ensuring path accuracy, reduce the sudden change problems of speed and acceleration, and thus improve the smoothness and equipment life of the picking robotic arm during execution; by setting the time scale as a quintic polynomial function and combining a fifteen-degree composite function to derive the trajectory speed and acceleration expressions, it is possible to achieve unified control of the trajectory under time parameterization, facilitate precise scheduling and optimization in the time domain, and further enhance the flexibility of trajectory control and time control accuracy.

[0034] 6. Introducing an improved particle swarm optimization algorithm and dynamically adjusting the inertia weight and learning factor can effectively improve the convergence speed of the algorithm during the search process and its ability to jump out of local optima; optimizing with the shortest motion time of the robotic arm as the objective function makes the final trajectory take into account both time efficiency and mechanical constraints, significantly improving the response speed and energy utilization rate of the operation system. Description of the Drawings

[0035] To more clearly illustrate the technical solution of the present application, the accompanying drawings required in the description will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can be obtained based on these drawings.

[0036] Figure 1 Flow chart of the trajectory planning method for the picking robotic arm shown in some embodiments; Figure 2 Quintic polynomial interpolation trajectory diagram shown in some embodiments; Figure 3 Seventh polynomial interpolation trajectory diagram shown in some embodiments; Figure 4 Path planning comparison diagram shown in some embodiments; Figure 5 Calculation trajectory curve effect diagram shown in some embodiments; Figure 6 Rastrigin function fitness curve diagram shown in some embodiments. Detailed implementation manners

[0037] To make the application purpose, features, and advantages of the present application more obvious and understandable, the technical solution protected by the present application will be clearly and completely described below by using specific embodiments and the accompanying drawings. Obviously, the embodiments described below are only some embodiments of the present application, rather than all embodiments. Based on the embodiments in this patent, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of this patent.

[0038] With the development of modern agriculture, automated and intelligent picking technologies have gradually become the key means to improve fruit picking efficiency and reduce labor costs. Apples are the main fruit variety in China, with a concentrated picking period and high labor intensity, and the dependence on human resources in the picking link is particularly significant. In recent years, picking robots, as intelligent equipment integrating visual recognition, trajectory planning, and servo control, have become a hot research direction in agricultural engineering.

[0039] In the prior art, the path planning methods of picking robotic arms mainly include offline planning methods based on traditional kinematic models and online planning methods combined with environmental perception. Some technologies use path search algorithms such as A*, RRT (Rapidly-Exploring Random Trees), and DWA (Dynamic Window Approach) to achieve collision-free path planning from the starting point to the target point. However, such methods are difficult to balance the smoothness and time optimality of the path, and have a slow convergence speed and are prone to falling into local optima in complex environments. To achieve smooth tracking of the path, some studies introduce polynomial interpolation or spline fitting methods, but fail to fully consider the dynamic constraints of the robotic arm and the performance limitations of the actuators, resulting in difficult practical deployment of the trajectory.

[0040] In addition, for the path optimization problem, intelligent optimization algorithms such as Particle Swarm Optimization (PSO) are widely used in the motion planning of robotic arms. However, the original PSO algorithm has deficiencies in convergence speed, local escape ability, and dynamic adjustment, resulting in limitations in its adaptability and optimization efficiency in actual picking operations.

[0041] The present invention addresses the problems in the prior art and provides a trajectory planning method, device, terminal, and medium for a picking robotic arm, solving the problems of slow convergence speed and easy falling into local optima in the robotic arm trajectory planning scheme in complex environments.

[0042] As Figure 1 shown, the present application provides a trajectory planning method for a picking robotic arm, including 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, element models that can collide in the scene, non-picking target models, and picking target models; Step S2: Based on the established three-dimensional model, construct a collision detection model for the robotic arm during operation, and determine a collision-free planned path based on the three-dimensional model and the collision detection model; Step S3: According to the joint angles, angular velocities, and angular accelerations of the starting point and the ending point of the planned path determined in Step S2, use a fifth-degree polynomial interpolation function to preliminarily fit the joint trajectory of the robotic arm, and determine the fifth-degree polynomial preliminary trajectory function of the robotic arm on the planned path; Step S4: Use a fifth-degree polynomial function as the time scale function, and use a cubic B-spline curve to smooth and optimize the preliminary trajectory function of the planned path to construct a calculated trajectory function of the picking robotic arm; Step S5: Taking the shortest motion time of the picking robotic arm as the optimization goal, use an improved particle swarm algorithm to optimize the calculated trajectory function under the constraint conditions of the rated angular velocity, rated angular acceleration, and rated torque of the robotic arm to obtain an optimized trajectory function of the picking robotic arm with optimal time.

[0043] In some embodiments, the geometric models of the robotic arm and the fruit tree branches are constructed using the CBB envelope box method. According to the actual situation of the robotic arm in the picking scenario, the link 1 will not collide with obstacles. Therefore, only the models of links 2-6 need to be constructed. Additionally, the relative position of link 6 changes little during the movement of the robotic arm, and a cylinder is used to replace link 6 to further simplify the model; To facilitate mechanized fruit picking, the fruit tree type adopts the trellis cultivation mode; the geometric model of non-target fruits is constructed using the SBB envelope box method; In some embodiments, according to the models of the robotic arm and obstacles constructed above, the collision detection situations are divided into two types, namely, the collision detection between the robotic arm and the fruit tree branches and the collision detection between the robotic arm and non-target fruits, which can be equivalent to the collision detection between cylinders and the collision detection between cylinders and spheres; 、 、 respectively represent the radii of the robotic arm link model, the fruit tree branch model, and the non-target fruit model. AB represents the axis of the link model, CD represents the axis of the fruit tree branch model, and O represents the center of the sphere of the non-target fruit model. represents the distance between AB and CD, represents the distance between AB and O. When , there is no collision between the robotic arm link and the fruit tree branch; when , there is no collision between the robotic arm link and the non-target fruit.

[0044] In some embodiments, the robotic arm collision detection algorithm is used to obtain the of the robotic arm. Since the 6 joints of the robotic arm have little influence on the position of the robotic arm link model, the collision detection is mainly performed on the states of the first three joints of the robotic arm.

[0045] In some embodiments, step S2 includes the following steps: Step S2-1: Based on the established three-dimensional model, construct the collision detection model of the robotic arm during the operation process. The collision detection model includes the boundary description of the obstacle, the motion constraints of the robotic arm, and the collision determination rules; Step S2-2: Model the action control process of the robotic arm as a decision model based on state, action, state transition, and reward and 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.

[0046] In some embodiments, the robotic arm joint angle using the quintic polynomial interpolation function is Function: (1) In the formula are the unknown coefficients of the fifth - order interpolation function.

[0047] Then the joint angular velocity and joint angular acceleration of the robotic arm are expressed as follows: (2) The fifth - order polynomial interpolation trajectory constrains the starting and ending joint angles, joint angular velocities, and joint angular accelerations. That is, the starting and ending times of the robotic arm movement are and , then the joint angle values, joint angular velocity values, and joint angular accelerations at the starting and ending moments are respectively: (3) Substituting Equation (3) into Equations (1) and (2), the trajectory function of the robotic arm can be obtained. Solving for the unknown coefficients in Matlab gives the fifth - order polynomial interpolation trajectory, as shown in Figure 6 Figure.

[0048] It can be seen from the figure that the angular velocity curve has a smooth profile and no inflection points, and the angular acceleration curve is continuous, avoiding joint vibrations caused by sudden changes in angular acceleration, enabling the robotic arm to pass through each path point smoothly.

[0049] However, the joint angle curve of the robotic arm deviates from the original path. Although the joint acceleration curve of the robotic arm is continuous, its curve profile is not smooth enough and the numerical change range is large. The large - range acceleration and deceleration of the joint motor in a short time will cause excessive inertial forces on it, resulting in vibrations during the operation of the robotic arm and reducing the service life of the robotic arm. Therefore, a higher - order polynomial interpolation function is needed to represent the joint trajectory. The higher the order, the smoother the joint acceleration curve of the robotic arm, which is beneficial to the smooth movement of the robotic arm.

[0050] The joint angle of the robotic arm using a seventh - order polynomial interpolation function is a function of time (4) In the formula are the unknown coefficients of the seventh - order interpolation function.

[0051] It can be seen from Equation (4) that the seventh - order interpolation function contains 8 unknown coefficients, so 8 constraint conditions are needed for solving. Therefore, in addition to the joint angle, joint angular velocity, and joint angular acceleration of the robotic arm, constraints need to be imposed on its joint jerk. For the joint angle of the robotic arm, joint angular velocity 、Joint angular acceleration and joint angular jerk are respectively expressed as: (5) Similarly, for the start and end times and of the manipulator movement, the joint angles, joint angular velocities, joint angular accelerations, and joint angular jerks at the start and end moments are respectively: (6) Substituting Equation (6) into Equations (4) and (5), the seven-degree polynomial trajectory function of the manipulator can be obtained.

[0052] Solving for the unknown coefficients in Matlab gives the seven-degree polynomial interpolation trajectory, as Figure 6 shown.

[0053] It can be seen from the figure that compared with the five-degree polynomial interpolation trajectory, for the seven-degree polynomial interpolation trajectory, the joint angle curve of the manipulator deviates less from the original path, and the joint velocity curve and joint acceleration curve are smoother and have no inflection points. The change range of the joint acceleration is smaller than that of the five-degree polynomial interpolation trajectory, improving the problem in the five-degree polynomial trajectory where the excessive change range of the joint acceleration causes the manipulator to shake and deviate severely from the original path.

[0054] Therefore, the seven-degree polynomial interpolation trajectory is more suitable for the trajectory planning of the manipulator.

[0055] The higher the order of the interpolation function of the manipulator joint trajectory, the better the running effect of the planned trajectory. However, the coefficients of the polynomial interpolation function also increase with the increase of the order, and more constraint conditions are required to solve the unknown coefficients, making the solution more difficult.

[0056] To avoid the picking manipulator from colliding with obstacles, obtain a smooth trajectory function, and reduce the solution difficulty, this application uses a cubic B-spline function to optimize the path and uses a five-degree polynomial interpolation function as the expression of the time scale in the cubic B-spline function, thereby obtaining the trajectories of each joint of the picking manipulator.

[0057] Aiming at the problem that the path generated by the Q-learning algorithm is not smooth, this application uses a B-spline curve to optimize the path. The B-spline curve is a common representation method of free-form curves in computer graphics and is a polynomial curve that approximates its control points. The expression of the k-th order B-spline curve is: (7) In the formula, is the joint angle vector, is a control point, representing a path point in this application; u is the time scale, and its value range is [0, 1]; is the basis function of the B-spline curve, and its expression is: (8) In the formula, k is the B-spline degree, is the knot, and the number of knots is m + k + 1. The fitting effect of the B-spline curve is as Figure 6 shown. It can be seen from the figure that the cubic B-spline is closer to the path obtained by path planning than the quartic B-spline curve and the quintic B-spline curve. Therefore, this application selects the cubic B-spline curve to fit the path.

[0058] To avoid impacting the joint motors, the trajectory of the robotic arm should be a sufficiently smooth function of time and needs to comply with constraints such as joint velocity, acceleration, or torque. However, the cubic B-spline curve cannot meet the requirements.

[0059] Let the time scale u be a quintic polynomial about time t, and its expression is: (9) Its first derivative and second derivative are: (10) (11) Substitute Equation (9) into Equation (7) to get: (12) In the formula, is a fifteen-degree polynomial function, and its expression is: (13) Among them, are the coefficients of the polynomial.

[0060] Observing Equations (12) and (13), it can be seen that the unknown coefficients of the picking robotic arm trajectory function are constants about , so only need to solve to determine its trajectory function. Referring to the method of solving the coefficients of the quintic polynomial, that is, the start and end times and of the picking robotic arm movement, then the time scales, joint angular velocities, and joint angular accelerations at the start and end times are: (14) Differentiate Equation (12) with respect to time t to obtain the expression of the joint angular velocity of the picking robotic arm as: (15) In the formula, , and its expression is: (16) Similarly, taking the second derivative of Equation (12) with respect to time t, the expression for the joint angular acceleration of the picking robotic arm is obtained as: (17) In the formula, , and its expression is: (18) Substitute the joint angular velocities of the picking robotic arm at the start and end times into Equation (15), and transform it to get: (19) Substitute the joint angular accelerations of the picking robotic arm at the start and end times into Equation (17), and transform it to get: (20) Substitute Equation (14), Equation (19), and Equation (20) into Equation (9), Equation (10), and Equation (11) to get: (21) Solve Equation (21) to obtain the expression for the unknown coefficient as: (22) Substitute Equation (22) into Equation (9), Equation (10), and Equation (11) to obtain the expressions of the time scale and its first and second derivatives with respect to time. Further substitute them into Equation (12), Equation (15), and Equation (17) to obtain the expressions of the joint angle, joint angular velocity, and joint angular acceleration of the picking robotic arm.

[0061] To visually display the effect of the fifteenth-degree polynomial trajectory obtained in this application based on B-spline and polynomial functions, the fifteenth-degree polynomial trajectory and the seventh-degree polynomial interpolation trajectory are respectively simulated and compared in a two-dimensional map. The simulation results are as Figure 6 shown.

[0062] It can be seen from the figure that the fifteenth-degree polynomial trajectory is smoother and closer to the original path than the seventh-degree polynomial interpolation trajectory. This is because in this application, the path is fitted by a cubic B-spline function, enabling the joints to cooperate with each other, resulting in a smaller deviation of the actual path from the original path. While the seventh-degree polynomial interpolation method interpolates each joint independently and cannot determine the actual path between two interpolation points, so the deviation from the original path is larger.

[0063] This application will use the trajectory planning method based on B-spline and polynomial functions and the seventh-degree polynomial interpolation method to respectively perform trajectory planning on the simulation path obtained in Section 3.4. Table 1 shows the simulation path points of the picking robotic arm. The obtained trajectory planning results of the picking robotic arm are as Figure 6 shown.

[0064] Table 1

[0065] To improve the working efficiency of the picking manipulator, it should work at the maximum power within the rated angular velocity, rated angular acceleration and rated torque ranges. Therefore, this application searches for the trajectory with the shortest movement time of the picking manipulator within the constraint range through an optimization algorithm.

[0066] The Particle Swarm Optimization (PSO) algorithm is a heuristic optimization algorithm that simulates the foraging behavior of birds. Due to its simple algorithm structure and high efficiency and stability, the PSO algorithm has been widely used in various optimization problems. In the PSO algorithm, a particle swarm containing N particles is first defined, and the particles are randomly distributed in the solution space. Each particle has two attributes, position and velocity, and a fitness value determined by the objective function, and the fitness value reflects the quality of the particle position. According to the individual optimal position and the global optimal position of the current particle, the moving speed and position of each particle are iteratively adjusted. The iterative formula is as follows: (23) In the formula, is the inertia weight; , are the individual learning factor and the global learning factor; is the position of the i-th particle; is the velocity of the i-th particle; is the optimal position of the i-th particle; is the optimal position of all particles; , are arbitrary values between [-1, 1].

[0067] However, since the inertia weight and the learning factors , of the traditional PSO algorithm are constants, it is easy to fall into the local optimum and it is impossible to balance the convergence speed and the convergence accuracy. Therefore, this application dynamically adjusts the inertia weight and the learning factors. The dynamic adjustment formulas for the improved inertia weight , and the learning factors , are: (24) In the formula, and are the maximum and minimum values of the inertia weight respectively; and are the initial value and the final value of the individual learning factor; and are the initial value and the final value of the population learning factor; is the current iteration number; is the maximum iteration number.

[0068] As can be seen from Equation (28), as the number of iterations increases, and gradually decrease, gradually increase, the global search ability of the algorithm weakens, and the local search ability enhances. Therefore, the improved PSO algorithm can achieve a balance between the global search ability and the local search ability, improving the overall optimization performance of the algorithm.

[0069] In order 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: (25) When x = 0 and y = 0, the Rastrigin function has the optimal fitness value , and the iterative fitness curves of the two particle swarm optimization algorithms are as Figure 6 shown: The improved PSO algorithm is significantly faster in convergence speed than the traditional PSO algorithm. The improved PSO algorithm is close to the optimal position when iterating to the 25th time, while the fitness value of the traditional PSO algorithm converges to the optimal value when iterating to the 47th time.

[0070] This application takes time as the optimization goal and uses the improved PSO algorithm to find the trajectory function with the shortest movement time of the picking manipulator. Since the path function is mainly determined by the first three joints, this application only considers the trajectory optimization of its first three joints. According to the structure of the picking manipulator in this application, the motor models of the first three joint modules are the same, and the rated angular velocity, rated angular acceleration, and rated torque of the motor are respectively: (26).

[0071] In some embodiments, this application provides a picking manipulator trajectory planning device, including: A three-dimensional modeling module, configured to collect data of the target environment and establish a three-dimensional model of the target environment; A collision detection and path planning module, configured to perform obstacle detection in the three-dimensional model and plan a collision avoidance path from the initial position of the manipulator to the target position according to the detection result and the kinematic model of the manipulator; A trajectory fitting module, configured to perform curve fitting or interpolation on the collision avoidance path to generate a continuous and smooth motion trajectory; A trajectory optimization module, which is used to optimize the motion trajectory on the premise of satisfying the kinematic and dynamic constraints of the robotic arm; A time optimization module, and the time optimization module based on the improved particle swarm optimization algorithm is used to optimize the time parameters of the optimized motion trajectory by using the improved particle swarm optimization algorithm to minimize the total time for the robotic arm to complete the trajectory.

[0072] In some embodiments, the present application provides a terminal, including: A memory, which is used to store the trajectory planning program of the picking robotic arm; A processor, which is used to implement the steps of the picking robotic arm trajectory planning method as described in the first aspect when executing the picking robotic arm trajectory planning action.

[0073] In some embodiments, the present application provides a computer-readable storage medium. The storage medium stores computer instructions. When a computer reads the computer instructions in the storage medium, the computer executes the described picking robotic arm trajectory planning method.

[0074] The basic concepts have been described above. Obviously, for those skilled in the art, the above detailed disclosure is only an example and does not constitute a limitation to this specification. Although not explicitly stated here, those skilled in the art may make various modifications, improvements, and corrections to this specification. Such modifications, improvements, and corrections are proposed in this specification, so such modifications, improvements, and corrections still belong to the spirit and scope of the exemplary embodiments of this specification.

[0075] In addition, unless clearly stated in the claims, the order of the processing elements and sequences described in this specification, the use of numbers, letters, or other names, is not used to limit the order of the processes and methods in this specification. Although some currently considered useful invention embodiments are discussed through various examples in the above disclosure, it should be understood that such details only serve the purpose of illustration. The appended claims are not limited to the disclosed embodiments. On the contrary, the claims are intended to cover all modifications and equivalent combinations that conform to the essence and scope of the embodiments of this specification. For example, although the system components described above can be implemented by hardware devices, they can also be implemented only through software solutions, such as installing the described system on existing servers or mobile devices.

[0076] Similarly, it should be noted that, in order to simplify the expression of the disclosure of this specification and thus help the understanding of one or more invention embodiments, in the description of the embodiments of this specification above, sometimes multiple features are merged into one embodiment, drawing, or description thereof. However, this disclosure method does not mean that the features required by the object of this specification are more than the features mentioned in the claims. In fact, the features of the embodiments are less than all the features of the single embodiments disclosed above.

[0077] In some embodiments, numbers are used to describe components and the quantity of attributes. It should be understood that such numbers used in the description of embodiments are, in some examples, modified by the modifiers "about", "approximately" or "substantially". Unless otherwise specified, "about", "approximately" or "substantially" indicate that the said numbers allow a variation of ±20%. Accordingly, in some embodiments, the numerical parameters used in the specification and claims are approximate values, and such approximate values may vary according to the characteristics required by individual embodiments. In some embodiments, the numerical parameters should consider the specified significant digits and adopt the method of retaining general digits. Although the numerical ranges and parameters used in some embodiments of this specification to confirm the breadth of their scope are approximate values, in specific embodiments, such numerical settings are made as precise as possible within the feasible range.

[0078] Finally, it should be understood that the embodiments described in this specification are only used to illustrate the principles of the embodiments of this specification. Other variations may also fall within the scope of this specification. Therefore, by way of example and not limitation, alternative configurations of the embodiments of this specification may be regarded as consistent with the teachings of this specification. Accordingly, the embodiments of this specification are not limited to the embodiments explicitly introduced and described in this specification.

Claims

1. A trajectory planning method for a picking robotic arm, characterized in that, Including 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, element models that can collide in the scene, non-picking target models, and picking target models; Step S2: Based on the established three-dimensional model, construct a collision detection model for the robotic arm during operation, and determine a collision-free planned path based on the three-dimensional model and the collision detection model; Step S3: According to the joint angles, angular velocities, and angular accelerations at the start and end points of the planned path determined in Step S2, use a fifth-degree polynomial interpolation function to preliminarily fit the joint trajectory of the robotic arm, and determine the fifth-degree polynomial preliminary trajectory function of the robotic arm on the planned path; Step S4: Use a fifth-degree polynomial function as the time scale function, and use a cubic B-spline curve to smooth and optimize the preliminary trajectory function of the planned path to construct the calculated trajectory function of the picking robotic arm; Step S5: Taking the shortest movement time of the picking robotic arm as the optimization goal, use an improved particle swarm algorithm to optimize the calculated trajectory function under the constraint conditions of the rated angular velocity, rated angular acceleration, and rated torque of the robotic arm to obtain the time-optimal optimized trajectory function of the picking robotic arm.

2. The trajectory planning method of the picking robotic arm according to claim 1, characterized in that In Step S1, the CBB envelope box method is used to construct the robotic arm link model and the element models that can collide; the SBB envelope box method is used to construct the non-picking target models.

3. The trajectory planning method of the picking robotic arm according to claim 1, wherein, Step S2 includes the following steps: Step S2-1: Based on the established three-dimensional model, construct a collision detection model for the robotic arm during operation. The collision detection model includes the boundary description of obstacles, the motion constraints of the robotic arm, and the collision determination rules; Step S2-2: Model the action control process of the robotic arm as a decision-making model based on state, action, state transition, and reward and punishment mechanisms; Step S2-3: Use the reinforcement learning Informed-Q algorithm to train the decision-making model of the robotic arm to obtain a path planning model, and determine a collision-free planned path based on the path planning model.

4. The trajectory planning method of the picking robotic arm according to claim 1, wherein In step S3, the robotic arm joint angle using a fifth-degree polynomial interpolation function is a function of time as follows: （1） where are the unknown coefficients of the fifth-order interpolation function; The joint angular velocity of the robotic arm and the joint angular acceleration are expressed as follows: （2） The fifth-degree polynomial interpolation trajectory constrains the starting and ending joint angles, joint angular velocities, and joint angular accelerations, that is, the starting and ending times of the manipulator motion are and , then the joint angle values, joint angular velocity values, and joint angular accelerations at the starting and ending moments are respectively: （3） Substituting Equation (3) into Equations (1) and (2) can obtain the preliminary trajectory function of the robotic arm.

5. The trajectory planning method of the picking robotic arm according to claim 4, wherein In Step S4, the expression of the k-th B-spline curve is: Wherein, is the joint angle vector, is the control point, which represents the path point in this application; u is the time scale, and its value range is [0, 1]; is the basis function of the B-spline curve, and its expression is: where k is the B-spline degree and k = 3, are the knots, and the number of knots is m + k + 1.

6. The trajectory planning method of the picking robotic arm according to claim 5, characterized in that Let the time scale u be a fifth-degree polynomial about time t, and its expression is: Its first derivative and second derivative are: Obtained: In the formula, is a fifteenth-degree polynomial function, and its expression is: Among them, is the coefficient of the polynomial; The unknown coefficients of the trajectory function of the picking manipulator are constants with respect to Solve to determine the calculated trajectory function, that is, the start and end times of the movement of the picking manipulator and , then the time scale, joint angular velocity, and joint angular acceleration at the start and end times are: The expression of the joint angular velocity of the picking robotic arm is: ； In the formula, ; The expression of the joint angular acceleration of the picking robotic arm is: ； In the formula, ; Substitute the joint angular velocities of the picking robotic arm at the start and end times into the expression of the joint angular velocity, and transform to get: Substitute the joint angular accelerations of the picking robotic arm at the start and end times into the expression of the joint angular acceleration, and transform to get: Calculating unknown coefficients The expression for Calculate the expressions of the joint angles, joint angular velocities, and joint angular accelerations of the picking robotic arm, which are the calculated trajectory functions.

7. The trajectory planning method of the picking robotic arm according to claim 1, wherein In Step S5, define a particle swarm containing N particles, randomly distribute the particles in the solution space, and iteratively adjust the moving speed and position of each particle. The iterative formula is as follows: In the formula, is the inertia weight; , are the individual learning factor and the swarm learning factor; is the position of the i-th particle; is the velocity of the i-th particle; is the optimal position of the i-th particle; is the optimal position of all particles; , are arbitrary values between [-1, 1]; Dynamically adjust the inertia weight and learning factors to obtain an improved PSO algorithm. The improved inertia weight , learning factor 、 The dynamic adjustment formulas are as follows: Wherein, and are respectively the maximum value and the minimum value of the inertia weight; and are the initial value and the final value of the individual learning factor; and are the initial value and the final value of the swarm learning factor; is the current iteration number; is the maximum iteration number; This application aims to optimize time. It uses an improved PSO algorithm to find the optimized trajectory function with the shortest movement time for the picking robotic arm. Considering the trajectory optimization of the first three joints, the rated angular velocity, rated angular acceleration, and rated torque of the motor are respectively: 。 8. A trajectory planning device for a picking robotic arm, characterized in that, Including: A three-dimensional modeling module, which is used to collect data of the target environment and establish a three-dimensional model of the target environment; A collision detection and path planning module, which is used to detect obstacles in the three-dimensional model and plan a collision avoidance path from the initial position of the robotic arm to the target position according to the detection results and the kinematic model of the robotic arm; A trajectory fitting module, which is used to perform curve fitting or interpolation on the collision avoidance path to generate a continuous and smooth motion trajectory; A trajectory optimization module, which is used to optimize the motion trajectory on the premise of meeting the kinematic and dynamic constraints of the robotic arm; A time optimization module, and the time optimization module based on the improved particle swarm algorithm is used to optimize the time parameters of the optimized motion trajectory by using the improved particle swarm algorithm to minimize the total time for the robotic arm to complete the trajectory.

9. A terminal, characterized in that, Including: A memory, which is used to store the trajectory planning program of the picking robotic arm; A processor, which is used to implement the steps of the picking robotic arm trajectory planning method described in any one of claims 1-7 when performing the picking robotic arm trajectory planning action.

10. 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 picking robotic arm trajectory planning method described in any one of claims 1 to 7.

Images