A motion planning and interference discrimination method for a double-truss palletizing robot

Abstract

A motion planning and interference discrimination method for a double-truss palletizing robot relates to the technical field of robot motion planning. Analysis variables are constructed based on an S-shaped speed control curve; standardized planning of variables is performed in combination with dynamic response analysis; a feasibility matrix is established to perform mathematical operations on motion parameters and reconstruct the control curve to complete preliminary motion planning; a determination is made as to whether the robot is stagnant; if so, the feasibility is reallocated for preliminary motion planning; if not, interference discrimination is performed; if no interference exists, the motion plan is optimized and evaluated; if so, interference correction factors are introduced to restrict the motion parameters; motion parameters in non-optimal motion planning are processed based on dichotomy and optimized values until an optimal solution is output. A feasibility value is constructed based on the relative position of the two robots for discrimination, and the S-shaped speed control curve is used as a basis to incorporate the feasibility analysis into the robot's motion planning, thereby improving the operating efficiency of the double-truss palletizing robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion planning, in particular to a double-truss palletizing robot motion planning and interference discrimination method. Background Art

[0002] In the wooden door and window industry, the trend toward customization has brought greater opportunities and benefits to companies, but it has also made it impossible for traditional production lines to further meet the diverse orders and flexible product production required for customization. In the production of passive doors and windows, the sorting, transportation, and stacking of door and window frames of varying sizes are a major challenge within the industry. Truss manipulators offer advantages such as a simple structure, a wide operating range, and easy control. They are often used for loading and unloading, handling, spraying, welding, and assembly. Considering the introduction of truss manipulators combined with actual working conditions to design solutions for frame material sorting, screening, transportation, and stacking becomes feasible. During operation, the manipulator's motion characteristics and trajectory are closely related to the efficiency of frame material transportation and the quality of stacking. Therefore, studying manipulator motion planning and analyzing its motion characteristics are of great significance.

[0003] In the study of motion planning for truss manipulators, Zeng first used the Monte Carlo method to obtain the collaborative workspace of the manipulator as the boundary equation to give the motion trajectory constraints. Li proposed a segmented acceleration planning method combined with angle and distance observers to perform particle swarm optimization on the workspace, which can enable the manipulator to reach the set position in the shortest time. Marco describes and controls the motion characteristics of the flexible body during movement with B-spline curves, aiming to reduce the residual vibration in the flexible system, which has certain significance for the vibration reduction motion analysis of the manipulator with a slender structure. Ding Xin used S-curves, sinusoidal acceleration and deceleration curves and fifth-order polynomials to control the motion of the truss manipulator. Comparative experiments showed that curve control based on fifth-order polynomials can make the manipulator motion more stable and reliable. Bao Zhongfu used point drive and spline function drive to solve the spatial spiral trajectory problem of the manipulator and established related models. Although the above scholars have conducted a series of studies on the motion planning of truss manipulators, most of them were conducted in a single truss system or a single coordinate system environment. Moreover, the movement of the truss manipulator requires different power sources and different transmission systems to be included in the analysis scope. The system responses generated by different structures will also be different. The periodic form of dynamic loads and modal analysis are helpful to improve the transmission efficiency and accuracy of the manipulator.

[0004] According to the actual needs of enterprises, the present invention takes the double-truss palletizing robot of the current production line as the research object, combines the actual door and window material transportation working conditions, analyzes the various motion states of the double-truss palletizing robot in the linkage process, uses the S-shaped curve as the basic control curve to optimize the trajectories of different motion modes, and uses ADAMS and Simulink to perform virtual simulation to solve the optimized trajectory, which can provide a theoretical basis for the motion control of the double-truss palletizing robot. Summary of the Invention

[0005] In order to address the shortcomings of the background technology, the present invention provides a double-truss palletizing robot motion planning and interference discrimination method, which constructs a feasibility value based on the relative positions of the two robots for discrimination, and incorporates the feasibility analysis into the robot motion planning based on the S-shaped speed control curve, which can significantly improve the operating efficiency of the double-truss palletizing robot.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: a motion planning and interference discrimination method for a double-truss palletizing robot, comprising the following steps:

[0007] S01. Based on the double-truss palletizing robot, the robot motion is planned for the palletizing scenario where there is a collision and interference problem between the two robots.

[0008] S02. Select the S-type speed control curve to plan the motion of the two manipulators. The S-type speed control curve is divided into seven time zones: acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration. There are also five speed variables: the maximum speed v1 of the acceleration zone, the maximum speed v2 of the uniform acceleration zone, the maximum speed v3 of the S-type speed control curve, the maximum speed v1' of the deceleration zone, and the maximum speed v'2 of the uniform deceleration zone. The seven time zones are defined in sequence as the time variable Δt. k k=1,2,...,7 means the maximum speed v3 of the S-type speed control curve is combined with the time variable Δt k It is possible to characterize the remaining velocity parameters, so the analysis variable Δt is constructed in the manipulator motion planning k and v3 as motion parameters;

[0009] S03. Preliminary determination of Δt based on dynamic response analysis k The range of k≠4 and v3 is selected and one of them is taken as the standard value to complete the standardized planning of the manipulator. Through this standard value, k≠4, Δt4 and As motion parameters, k≠4 and is a given constant, and Δt4 is a variable calculated according to different displacement values of the manipulator;

[0010] S04, establish the feasibility matrix of motion parameters through the relative motion state of the two manipulators k≠4, Δt4 and Perform mathematical operations to reconstruct the S-shaped speed control curve to complete the preliminary motion planning of the double-truss palletizing robot;

[0011] S05: First, determine whether the two manipulators have stagnant motion based on the preliminary motion plan. If either manipulator has stagnant motion, proceed to step S04 to reallocate feasibility and perform preliminary motion planning. If not, determine the motion plan and proceed to step S06 for interference determination.

[0012] S06: Determine whether there is an interference area between the two manipulators. If not, proceed to step S07 to optimize and evaluate the motion plan. If so, introduce the interference correction factor λ i and β i The motion parameters are restricted, and the interference correction adopts the iterative method to correct the motion parameters Δt obtained in the first two times. k After performing mathematical operation correction with v3, the process goes to step S04;

[0013] S07, based on dichotomy and optimized numerical c k The motion parameter Δt in the non-optimal motion planning k After processing with v3, the process goes to step S04, and the motion parameter Δt corresponding to the initial value is iterated. k It is also defined as a non-optimal solution motion as v3, and then the difference in motion parameters before and after processing is used as the convergence criterion of iteration until the time variable Δt before and after processing is k If the absolute values of the differences are both less than 0.01 and the absolute values of the speed variable v3 differences are both less than 0.001, they are considered to be optimal solutions and output.

[0014] Compared with the prior art, the beneficial effects of the present invention are as follows: in view of the fact that there are multiple motion states of the double-truss manipulator, including interference, the present invention proposes to construct a feasibility value based on the relative positions of the two manipulators for identification, and formulates planning strategies for different motions. The S-shaped speed control curve is used as the basis and the feasibility analysis is incorporated into the motion planning of the manipulator to obtain a standardized motion model and a feasibility planning model. The displacement data is selected as the initial analysis element to solve the model and obtain all the time variables and speed variables in the planned motion. The data is then simulated and verified using ADAMS and Simulink, which shows that the planning model constructed using feasibility analysis is significantly more efficient than the unplanned model, and can avoid adverse conditions such as interference and manipulator stagnation, and the motion curve is smooth. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a structural diagram of a double-truss palletizing robot;

[0016] Figure 2 is a strategy diagram of the motion planning method of the present invention;

[0017] Figure 3 It is an S-type speed control curve;

[0018] Figure 4 This is a schematic diagram of two palletizing scenarios using a double-truss palletizing robot;

[0019] Figure 5 Schematic diagram of the starting and ending positions of the manipulator in the embodiment;

[0020] Figure 6 is a Simulink system block diagram in the embodiment;

[0021] Figure 7 2. The displacement comparison diagram of the manipulator under the two motion schemes in the embodiment;

[0022] Figure 8 This is a waveform diagram showing the relationship between the number of material roots and efficiency of the analysis model in the embodiment;

[0023] Figure 9 3 is a comparison diagram of the speed and acceleration during the motion cycle of the manipulator in the embodiment.

[0024] In the figure: 1- silo platform, 2- X-axis slide module, 3- Y-axis slide module, 4- Z-axis slide module, 5- end effector, 6- pneumatic dead stop, 7- palletizing platform. DETAILED DESCRIPTION

[0025] The technical solutions of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0026] 1. Analysis of the working condition of the robot

[0027] 1.1 Structural plan

[0028] Combine Figure 1As shown, the conventional main structure of the double-truss palletizing robot mainly includes a silo platform 1 for carrying raw materials such as wood, and the wood can be positioned on its surface by arranging pneumatic dead stops 6 in a matrix. Two X-axis slide modules 2 are arranged side by side on both sides of the edge of the silo platform 1, and each X-axis slide module 2 is equipped with two X-axis sliders for controlling the movement of the double trusses along the X-axis direction. The double trusses use two Y-axis slide modules 3 arranged in parallel, and the two Y-axis slide modules 3 are respectively fixed between the two X-axis sliders corresponding to the two X-axis slide modules 2, and each Y-axis slide module 3 is equipped with a Y-axis slider for controlling the movement of the manipulator along the Y-axis direction. A Z-axis slide module 4 is fixed on each Y-axis slider, and each Z-axis slide module 4 is equipped with a Z-axis slider for controlling the movement of the manipulator along the Z-axis direction. An end effector 5 is fixed at the bottom of each Z-axis slider for grabbing the wood. By configuring the X-axis slide module 2, the Y-axis slide module 3, and the Z-axis slide module 4, both manipulators have three degrees of freedom, meaning they can move along the X, Y, and Z axes. To achieve division of labor and collaboration, the two manipulators can be divided into Manipulator No. 1 and Manipulator No. 2. Manipulator No. 1 is primarily responsible for transporting the wood blocks on the silo platform 1 to Manipulator No. 2, while Manipulator No. 2 is primarily responsible for stacking the blocks. At the same time, when the blocks are very close to Manipulator No. 2's hand position, Manipulator No. 2 can also directly grab and stack them. A stacking platform 7 can be placed at the bottom of the silo platform 1 near Manipulator No. 2 for stacking the blocks.

[0029] 1.2. Action status of dual manipulators

[0030] In a dual-robot environment, there is a common motion area between the two robots, which means that there is motion interference in some movements. The conventional solution to this situation is to solve it by having one of the robots avoid or wait, which sacrifices a certain amount of work efficiency to ensure stable operation of the system.

[0031] Since the motion interference mainly occurs when the two manipulators' X-axes approach each other in opposite directions, this solution mainly analyzes the linkage operation status of the two manipulators' X-axes and Z-axes, and defines a 6×6 matrix to express the manipulator's operability:

[0032]

[0033] The matrix is composed of feasibility P, which is distributed from 0 to 1 on the number axis, with 1 and 0 representing "runnable" and "not runnable" respectively. The matrix elements are represented by P ij The definition is as follows:

[0034] 0≤P ij ≤10≤i≤5,0≤j≤5i,j∈N

[0035] According to the motion state, the runnability P has the following relationship:

[0036]

[0037] According to the analysis of the robot's motion cycle, since the robot's Z-axis motion is performed when the X-axis is stopped, there is an inclusion relationship in the feasibility:

[0038]

[0039] Feasibility set P 1j and P i1 There are multiple feasibility values in it, which cannot be directly reflected in motion analysis. The eigenvalue is constructed by the subsets contained in it for quantitative representation. The eigenvalue is expressed as and

[0040]

[0041]

[0042] The feasibility value between 0 and 1 cannot be easily calibrated. Therefore, the definition of fuzzy mathematical membership is introduced and a function judgment formula is constructed to calculate the feasibility value:

[0043]

[0044] Where a and b represent the feasible degree set P 1j and P i1 Neutron element P ij The number of 1s.

[0045] According to formulas (2)-(5), the feasible quantization matrix A of the manipulator can be obtained:

[0046]

[0047] 2. Motion planning of double-truss palletizing robot

[0048] 2.1 Speed control curve

[0049] Common speed control curves for robots include trapezoidal curves, S-shaped curves, Bezier curves, and sinusoidal acceleration and deceleration curves. The S-shaped curve has continuous connections in different acceleration stages and a controllable acceleration rate, which reduces the impact on the robot and makes it widely used. Therefore, in the double-truss palletizing robot system, we use the S-shaped speed control curve as the basis for motion planning analysis.

[0050] Combine Figure 3As shown, first of all, the S-type speed control curve model is analyzed. The S-type speed control curve is mainly divided into seven stages: acceleration section, uniform acceleration section, deceleration section, uniform speed section, acceleration and deceleration section, uniform deceleration section, and deceleration and deceleration section. t0 represents the initial time, t1 represents the end time of the acceleration section, t2 represents the end time of the uniform acceleration section, t3 represents the end time of the deceleration section, t4 represents the end time of the uniform speed section, t5 represents the end time of the acceleration and deceleration section, t6 represents the end time of the uniform deceleration section, and t7 represents the end time of the deceleration and deceleration section. v1 represents the maximum speed of the acceleration section, v2 represents the maximum speed of the uniform acceleration section, v3 represents the speed of the uniform speed section, which is also the maximum speed of the S-type speed control curve, v1' represents the maximum speed of the deceleration section, and v'2 represents the maximum speed of the uniform deceleration section.

[0051] Most studies on S-shaped speed control curves use symmetric acceleration and deceleration to build models. This results in only three variables on the speed axis, making it suitable for motion control in simple systems. However, for dual-truss palletizing robots, which face multiple constraints, a symmetrical approach to maintaining the same acceleration and deceleration strategy is undesirable. Different control curves must be planned for the acceleration and deceleration segments to ensure system stability, based on the different motions of the two robots.

[0052] In the S-shaped speed control curve, the acceleration is trapezoidally distributed, which can be characterized by constructing a proportional function, a constant function and a piecewise function. According to the differential relationship between speed and acceleration, the speed curve can be composed of a quadratic function, a proportional function, a constant function and a piecewise function:

[0053]

[0054] The relationship between each speed node:

[0055]

[0056] Speed (m / s):

[0057]

[0058] Acceleration (m / s 2 ):

[0059]

[0060] Displacement (m):

[0061]

[0062] Maximum displacement, maximum velocity, and maximum acceleration:

[0063]

[0064] 2.2 Planning Strategy

[0065] For the double-truss palletizing robot, its palletizing scenarios can be divided into two types: Scenario 1) The second robot will not take materials from the silo platform 1 and only performs palletizing actions. Figure 4 As shown in (a), the two manipulators move in opposite directions and continuously, and there is no waiting for each other; Scenario 2) As the materials are stacked higher and higher on the stacking platform, the stacking stroke of the second manipulator gradually shortens, and the stacking frequency increases, so that the second manipulator has no materials to stack. At this time, if the previous movement method is still used, one manipulator will inevitably wait for the other manipulator. As the number of layers increases, the waiting time will gradually increase, and the stacking efficiency will decrease. Therefore, it is necessary to plan the second manipulator to participate in the material silo platform 1 to pick up materials, and plan the first manipulator to go to a farther place to pick up materials. Figure 4 (b) to reduce waiting time.

[0066] Regarding scenario 2), the motion planning of the two manipulators in this scenario needs to solve three problems:

[0067] 1. Avoid collision and interference between two manipulators;

[0068] 2. Avoid the resonant frequency bands between the different motion modes of the two manipulators and control the motion mode in combination with dynamic response analysis;

[0069] 3. Efficiency is improved after planning in a single palletizing process.

[0070] The feasibility matrix can help us better analyze and solve these problems. Secondly, in the analysis of the S-type speed control curve of the manipulator, it can be seen that the motion equation is composed of various time variables and speed variables. There is a mapping relationship between each speed variable and the time variable. The maximum speed v3 of the S-type speed control curve is taken as the analysis target. The main analysis variables for the motion planning of the manipulator are:

[0071]

[0072] Where: R i is the total motion variable matrix of the two manipulators, T1 is the negative motion variable matrix of the manipulator in the X-axis, T2 is the negative motion variable matrix of the manipulator in the Z-axis, T3 is the positive motion variable matrix of the manipulator in the Z-axis, and T4 is the positive motion variable matrix of the manipulator in the X-axis.

[0073] Scenario 1) and a feasibility of 1 are the most common manipulator motions. The manipulator's acceleration and deceleration curves are consistent with the maximum speed. The uniform speed segment determines the total displacement of this segment. Therefore, the planning of this motion only involves a single variable, Δt4. The remaining variables in the curve equation are standardized and planned:

[0074]

[0075] Where, is a given constant, x max is the total displacement of the two manipulators in a single segment motion.

[0076] For the motion with feasibility between 0 and 1, the two manipulators are first standardized and planned using formula (14) as Then adjust the value of robot No. 1 according to the feasibility value. As the movement changes, robot No. 1 may have continuous feasibility changes in a single segment of movement. The time variable can be allocated first and then the speed variable can be planned.

[0077] The feasibility of a single segment remains unchanged:

[0078]

[0079] Single segment feasibility changes:

[0080]

[0081] Where x max1 Represents the total displacement of the current single-segment motion of the No. 1 manipulator, P is the feasibility value, and after the feasibility value is used to make a preliminary plan for the single-segment motion, the possibility of interference is considered in the total motion cycle. Define λ i , β i The interference correction factor is used to limit the motion variables, c k To optimize the value, then:

[0082]

[0083] Total motion interference discriminant:

[0084]

[0085] Where Δx is the total displacement difference of the two manipulators in the positive direction of the X-axis, h is the X-axis width of the manipulator, σ is the safety factor, sum(T j ) represents the sum of all elements in the matrix, dt is the starting time difference between the two manipulators, x(Δt,Δt k ,v3) represents the displacement of robot No. 1 from the end point after Δt time in the positive motion of X-axis.

[0086] For the motion with a feasibility of 0 (R1(T4), R2(T1)), we can first refer to the above 0-1 planning method to constrain it, approximately stop the manipulator, and then redistribute the feasibility value for planning:

[0087]

[0088] At this point, the motion planning analysis strategy of the double-truss palletizing robot is combined Figure 2 As shown, the process is as follows:

[0089] S01, two palletizing scenarios based on a double-truss palletizing robot. Scenario 2) is more efficient but has the problem of collision and interference between the two robots, requiring robot motion planning.

[0090] S02. Select the S-type speed control curve to plan the motion of the two manipulators. The S-type speed control curve is divided into seven time zones: acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration. There are also five speed variables: the maximum speed v1 of the acceleration zone, the maximum speed v2 of the uniform acceleration zone, the maximum speed v3 of the S-type speed control curve, the maximum speed v1' of the deceleration zone, and the maximum speed v'2 of the uniform deceleration zone. The seven time zones are defined in sequence as the time variable Δt. k (k=1,2,...,7) represents the maximum speed v3 of the S-type speed control curve combined with the time variable Δt k It is possible to characterize the remaining velocity parameters, so the analysis variable Δt is constructed in the manipulator motion planning k and v3 as motion parameters;

[0091] S03. Preliminary determination of Δt based on dynamic response analysis k (k≠4) and v3 range values and take one of them as the standard value (reference value) to complete the standardized planning of the manipulator. The standard value can be represented as the motion variable in the work of the conventional manipulator, and the result is (k≠4), Δt4 and As motion parameters, (k≠4) and is a given constant, and Δt4 is a variable calculated according to different displacement values of the manipulator;

[0092] S04, establish the feasibility matrix of motion parameters through the relative motion state of the two manipulators (k≠4), Δt4 and v3 * Perform mathematical operations to reconstruct the S-shaped speed control curve to complete the preliminary motion planning of the double-truss palletizing robot;

[0093] S05. First, determine whether the two manipulators have stagnant motion (speed less than 0.01 m / s) for the preliminary motion plan. If either manipulator has stagnant motion, proceed to step S04 to reallocate feasibility and perform preliminary motion planning. If not, determine the motion plan and proceed to step S06 for interference determination.

[0094] S06: Determine whether there is an interference area between the two manipulators according to the total motion interference discriminant. If not, proceed to step S07 to optimize and evaluate the motion plan. If so, introduce the interference correction factor λ iand β i The motion parameters are restricted, and the interference correction adopts the iterative method to correct the motion parameters Δt obtained in the first two times. k After performing mathematical operation correction with v3, the process goes to step S04;

[0095] S07, based on dichotomy and optimized numerical c k The motion parameter Δt in the non-optimal motion planning k After processing with v3, the process goes to step S04, and the motion parameter Δt corresponding to the initial value is iterated. k It is also defined as a non-optimal solution motion as v3, and then the difference in motion parameters before and after processing is used as the convergence criterion of iteration until the time variable Δt before and after processing is k If the absolute values of the differences are both less than 0.01 and the absolute values of the speed variable v3 differences are both less than 0.001, they are considered to be optimal solutions and output.

[0096] Example

[0097] In order to verify the effectiveness of this solution more intuitively, ADAMS and MTALAB Simulink are used to build a simulation model for simulation verification. Figure 5 As shown, on the X-axis, the starting and ending coordinates of Robot 1 are consistent, denoted as Start 1 and End 1, respectively. The ending coordinate of Robot 2 is labeled End 2, which is 0.3m away from End 1. Robot 2's starting coordinates are not constant and change with the palletizing position. The distance between Robot 2's starting coordinates and Start 1 is defined as dx0, which is then substituted into the calculations in the detailed example. The five data sets in Table 1 were collected and compared with another scheme, which was a single-manipulator material retrieving motion cycle.

[0098] Table 1 Target displacement of the manipulator / m

[0099]

[0100] The gripping time of the robot is simulated with a delay of 3s. The model is built in Simulink environment and combined with Figure 6 As shown in the figure, the simulation step size is 0.02s, the total simulation time is 22s, and the first 5 seconds is the initial displacement adjustment time.

[0101] The X-axis motion of the manipulator marks the beginning and end of the entire motion cycle. The collision and interference between the two manipulators mainly occurs in this motion segment. Therefore, the analysis of the X-axis displacement of the manipulator is very important. Figure 7 As shown in the figure, it can be seen that by using the feasibility value to plan the motion trajectory of the manipulator, the motion trajectory of manipulator No. 1 in the five groups of test data has no intersection with manipulator No. 2. Figure 7As shown in (a), collision interference does not occur. Further optimization of the motion trajectory ensures that the average return time of the two manipulators to their respective endpoints is within 0.9 seconds. In the data environment with the shortest target displacement, both reach their endpoints within 1.3 seconds. The time margin gradually increases with increasing target displacement, demonstrating the desirability of planning manipulator motion based on constructing feasibility values.

[0102] For another solution, the No. 1 robot moves in a standardized manner. Figure 7 As shown in (b), the total time spent in data 1 is 15.38s, and the time spent in the planned movement is 17.82s. In terms of time, it takes less than the planned movement, but this advantage also decreases as the target displacement increases. In terms of the number of materials to be picked up, a single manipulator can only grab one material at a time, while a dual manipulator can grab two after planning, which reduces the stacking pressure on the silo platform. In terms of efficiency, considering that it is unlikely to plan the second manipulator to pick up materials from the silo platform for two consecutive movement cycles, the second manipulator cannot stack the two existing materials in a short time. We can use the interval method to divide the movement mode for the continuous cycle. For example, in the first cycle, the dual manipulator planning movement is adopted, the second or third cycle adopts the single manipulator movement, and the fourth cycle is the planned movement. In this way, the transportation of multiple materials is planned and controlled. In this mode, a simple efficiency analysis model can be constructed as follows:

[0103]

[0104] In the formula, N represents the number of material roots, t single is the single manipulator motion cycle time, t double is the cycle time of the dual-manipulator movement, floor means rounding the variable down, T1 represents the total transportation time of N materials according to the interval cycle, and T2 represents the total transportation time of N materials according to the single-manipulator movement.

[0105] Combined with simulation data, the material transportation efficiency of the dual manipulator motion planning is improved by about 28.064% compared with the single manipulator. The relationship between the number of material roots and efficiency of the analysis model is combined Figure 8 shown.

[0106] Combination of the speed and acceleration changes of the No. 1 robot during the entire motion cycle (without gripping time) Figure 9 As shown, Figure 9 The two largest peaks in (a) represent the X-axis velocity, and the smaller peak represents the Z-axis velocity. In the five data sets, when the target displacement value increases, the robot planning speed also increases. However, due to the constraint limit, it does not exceed the standardized value. The entire cycle process is relatively stable, without major inflection points or breakpoints. Figure 9 (b) is the corresponding acceleration change trend, the acceleration is mainly distributed at 2000mm / s2 Up to 4000mm / s 2 When the acceleration curve is not a trapezoidal distribution, a maximum value will appear, and there are step signals around the maximum value. The reference speed curve mainly occurs in the deceleration section, which indicates that the speed of the robot at the moment of stopping is quite different from that at the previous moment. In the future, we can focus on the optimization analysis of this motion segment and use higher-order algorithm logic and multiple constraints to solve it, so as to reduce the acceleration fluctuation.

[0107] In summary, the following conclusions can be drawn:

[0108] (1) Aiming at the actual needs of enterprises, the double-truss manipulator is taken as the research object, and the possible states of the double-truss manipulator during the movement process are analyzed. A method of constructing a feasibility analysis matrix for discrimination is proposed for the existing interference situations, and it is advocated to incorporate feasibility analysis into the motion planning of the manipulator;

[0109] (2) Based on the S-shaped speed control curve as the basic model and combined with feasibility analysis, the planning strategy and corresponding analysis model of the manipulator under various motion states were constructed, and the total motion interference discrimination equation of the manipulator was obtained as a constraint condition. The binary method was further used to extract the optimal solution variables from the model. Five groups of displacement data were selected as the initial analysis elements, and a program was written using MATLAB to solve all the time variables and speed variables in the planned motion.

[0110] (3) A simulation model was established using ADAMS and Simulink to perform numerical analysis and verification of the solution set. This demonstrated the effectiveness of the constructed model and further demonstrated the effectiveness of the method of using feasible value planning to construct a robot motion model. This simulation also simulated a single robot transport as a comparative test. The analysis showed that the dual robot transport efficiency after motion planning increased by approximately 28.064% compared to a single robot transport.

[0111] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other configurations without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims, not the foregoing description, and all variations coming within the meaning and range of equivalents of the claims are intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim to which it relates.

[0112] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A motion planning and interference discrimination method for a double-truss palletizing robot, characterized by: The following steps are involved: S01. Based on the double-truss palletizing robot, the robot motion is planned for the palletizing scenario where there is a collision and interference problem between the two robots. S02. Select the S-type speed control curve to plan the motion of the two manipulators. The S-type speed control curve is divided into seven time zones: acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration. There are also five speed variables: the maximum speed v1 of the acceleration zone, the maximum speed v2 of the uniform acceleration zone, the maximum speed v3 of the S-type speed control curve, the maximum speed v1' of the deceleration zone, and the maximum speed v'2 of the uniform deceleration zone. The seven time zones are defined in sequence as the time variable Δt. k k=1,2,...,7 means the maximum speed v3 of the S-type speed control curve is combined with the time variable Δt k It is possible to characterize the remaining velocity parameters, so the analysis variable Δt is constructed in the manipulator motion planning k and v3 as motion parameters; S03. Preliminary determination of Δt based on dynamic response analysis k The range of k≠4 and v3 is selected and one of them is taken as the standard value to complete the standardized planning of the manipulator. Through this standard value, Δt4 and As motion parameters, and is a given constant, and Δt4 is a variable calculated according to different displacement values of the manipulator; S04, establish the feasibility matrix of motion parameters through the relative motion state of the two manipulators Δt4 and Perform mathematical operations to reconstruct the S-shaped speed control curve to complete the preliminary motion planning of the double-truss palletizing robot; S05: First, determine whether the two manipulators have stagnant motion based on the preliminary motion plan. If either manipulator has stagnant motion, proceed to step S04 to reallocate feasibility and perform preliminary motion planning. If not, determine the motion plan and proceed to step S06 for interference determination. S06: Determine whether there is an interference area between the two manipulators. If not, proceed to step S07 to optimize and evaluate the motion plan. If so, introduce the interference correction factor λ i and β i The motion parameters are restricted, and the interference correction adopts the iterative method to correct the motion parameters Δt obtained in the first two times. k After performing mathematical operation correction with v3, the process goes to step S04; S07, based on dichotomy and optimized numerical c k The motion parameter Δt in the non-optimal motion planning k After processing with v3, the process goes to step S04, and the motion parameter Δt corresponding to the initial value is iterated. k It is also defined as a non-optimal solution motion as v3, and then the difference in motion parameters before and after processing is used as the convergence criterion of iteration until the time variable Δt before and after processing is k If the absolute values of the differences are both less than 0.01 and the absolute values of the speed variable v3 differences are both less than 0.001, they are considered to be optimal solutions and output.

2. The motion planning and interference discrimination method of a double-truss palletizing robot according to claim 1, characterized in that: The establishment of the feasibility matrix in S04 specifically includes: Based on the motion interference of the double-truss palletizing robot, the linkage operation status of the X-axis and Z-axis of the two robots is analyzed, and a 6×6 matrix is defined to express the operability of the robot: The matrix is composed of feasibility P, which is distributed from 0 to 1 on the number axis, with 1 and 0 representing executable and inoperable respectively. The matrix elements are represented by P ij The definition is as follows: 0≤P ij ≤10≤i≤5,0≤j≤5i,j∈N Subscript i reflects the movement of manipulator No. 1, subscript j reflects the movement of manipulator No. 2, i, j = 0 means movement along the -X axis, i, j = 1 means stationary on the X axis, i, j = 2 means movement along the +X axis, i, j = 3 means movement along the -Z axis, i, j = 4 means stationary on the Z axis, i, j = 5 means movement along the +Z axis; According to the motion state, the runnability P has the following relationship: According to the analysis of the robot's motion cycle, since the robot's Z-axis motion is performed when the X-axis is stopped, there is an inclusion relationship in the feasibility: The eigenvalue is constructed by the contained subsets for quantitative representation, and the eigenvalue is expressed as and The definition of fuzzy mathematical membership is introduced, and a function judgment formula is constructed to calculate the feasibility value: Where a and b represent the feasible degree set P 1j and P i1 Neutron element P ij is the number of 1s; Then we get the feasible quantization matrix A of the manipulator:

3. The motion planning and interference discrimination method of a double-truss palletizing robot according to claim 2, characterized in that: The mathematical operation in S04 specifically includes: Normalize the remaining variables in the curve equation: Where, is a given constant, x max is the total displacement of the single-segment motion of the two manipulators; For motions with feasibility between 0 and 1, the two manipulators are first standardized and planned using the above formula. Then, the value of manipulator No. 1 is adjusted according to the feasibility value. As the motion changes, the time variable of manipulator No. 1 is first allocated and then the speed variable is planned. The feasibility of a single segment remains unchanged: Single segment feasibility changes: Where x max1 represents the total displacement of the current single-segment motion of the No. 1 manipulator, P is the feasibility value, λ i and β i is the interference correction factor, c k To optimize the value; For a motion with a feasibility of 0:

4. The motion planning and interference discrimination method for a double-truss palletizing robot according to claim 1, characterized in that: The S06 determines whether there is an interference area between the two manipulators according to the total motion interference discriminant as follows: Where x(Δt,Δt k ,v3) represents the displacement of the No. 1 manipulator from the end point after Δt time in the positive motion of the X axis, Δx is the total displacement difference of the two manipulators in the positive motion of the X axis, h is the X axis width of the manipulator, σ is the safety factor, sum(T j ) represents the sum of all elements in the matrix, dt is the starting time difference between the two manipulators, R i is the total motion variable matrix of the two manipulators, T1 is the negative motion variable matrix of the manipulator in the X-axis, T2 is the negative motion variable matrix of the manipulator in the Z-axis, T3 is the positive motion variable matrix of the manipulator in the Z-axis, and T4 is the positive motion variable matrix of the manipulator in the X-axis.

5. A motion planning and interference discrimination method for a double-truss palletizing robot according to claim 1 or 3, characterized in that: The interference correction factor λ i and β i The calculation is as follows: Where P is the feasibility value.

6. The motion planning and interference discrimination method for a double-truss palletizing robot according to claim 5, characterized in that: The optimized value c k The following conditions must be met:

Images