Palletizing robot trajectory optimization method based on dynamic constraint

Abstract

The invention provides a robot palletizer trajectory optimization method based on dynamic constraints, the robot palletizer can execute multi-target boxing operation, and the optimization method comprises the following steps: S1, according to structural parameters of a mechanical arm, establishing a kinematic model and a dynamic model to obtain constraint conditions; s2, in combination with kinematics and dynamics constraint conditions, establishing an optimization function model by taking time and energy as optimization targets; s3, according to the optimization model, performing reconstruction optimization on the optimization model by using a convex optimization method so as to solve problems; s4, according to a single-target optimization problem, adding a torque change rate as a constraint condition, and realizing optimization of a multi-target function model in combination with the model to obtain an optimal track of multi-target boxing motion planning; according to the method, comprehensive optimization of time and energy can be considered, impact is reduced, and the optimal working track of the robot better meeting actual requirements is obtained.

Description

Technical Field

[0001] The invention relates to the technical field of palletizing robots, in particular to a palletizing robot trajectory optimization method based on dynamic constraints. Background Art

[0002] In the field of modern industrial automation, palletizing robots are widely used in various production lines to achieve efficient packing and palletizing operations. With the continuous improvement of production efficiency and safety requirements, research on trajectory optimization of palletizing robots has become particularly important. Especially in dynamic and complex working environments, traditional trajectory planning methods often cannot meet the dynamic constraints of the robot, thus affecting the overall operating performance.

[0003] At present, the path planning technology used in many palletizing robot designs mainly focuses on the optimization of geometric trajectories and relies on static models for planning, which makes it difficult to effectively reflect the dynamic behavior and physical limitations of the robot arm in actual operation. The shortcomings of existing technologies are mainly reflected in the following aspects:

[0004] 1. Insufficient consideration of dynamic constraints. Most existing trajectory planning methods fail to fully consider the kinematic and dynamic characteristics of the robotic arm, resulting in the inability to ensure the stability and safety of the robot under fast movement and heavy load conditions.

[0005] 2. Traditional methods usually perform poorly in complex packing environments, especially when performing tasks such as packing, turning and placing multiple items. Path calculations often contain redundancy and unnecessary movements, which affects overall work efficiency.

[0006] 3. Existing systems are usually designed for offline planning, which makes it difficult to respond to dynamically changing production environments in real time, such as the movement of surrounding obstacles, changes in cargo specifications, etc., resulting in the robot being unable to adjust its trajectory in time.

[0007] The robot's trajectory planning is mainly carried out in Cartesian space and joint space. There are many research results for single goals such as time optimization and energy optimization, and most of the research methods are based on the robot's kinematic constraints. In actual application scenarios, there are high requirements for the optimization of industrial robots in terms of time and energy consumption. Therefore, it is necessary to consider the comprehensive optimization of time and energy according to the actual working conditions of the robot, and reduce the impact to obtain the optimal trajectory that better meets actual needs.

[0008] It should be noted that the information disclosed in the above background technology section is only used for understanding the background of the present application, and therefore may include information that does not constitute prior art known to ordinary technicians in the field. Summary of the invention

[0009] The present invention proposes a palletizing robot trajectory optimization method based on dynamic constraints, which can consider the comprehensive optimization of time and energy and reduce impact, so as to obtain the optimal trajectory of the robot's work that better meets actual needs.

[0010] The present invention adopts the following technical solutions.

[0011] A palletizing robot trajectory optimization method based on dynamic constraints, the palletizing robot can perform multi-target boxing operations, the robot arm comprises a rotating frame (6) arranged at a base and capable of rotating horizontally; the rotating frame is provided with a telescopic arm assembly (8) driven by a telescopic arm lifting servo electric cylinder (7) to rotate vertically; the telescopic arm assembly is provided with a telescopic arm conveyor belt for conveying goods, and the end of the telescopic arm conveyor belt is provided with an end actuator (9) capable of rotating vertically;

[0012] The optimization method comprises the following steps:

[0013] Step S1, establishing a kinematic model and a dynamic model according to the structural parameters of the robot arm to obtain constraint conditions;

[0014] Step S2, combining kinematic and dynamic constraints, taking time and energy as optimization targets, and establishing an optimization function model;

[0015] Step S3: According to the optimization model, the optimization model is reconstructed and optimized by using a convex optimization method to solve the problem;

[0016] Step S4: according to the single-objective optimization problem, the torque change rate is added as a constraint condition, and the multi-objective function model is optimized in combination with the model to obtain the optimal trajectory of the multi-objective packing motion planning.

[0017] The base is a robot chassis (1) with a primary conveyor belt, and a chassis frame (2), an electric control cabinet (3), and counterweights (4) arranged on both sides of the chassis are provided on the chassis; the end of the primary conveyor belt is adjacent to the beginning of the telescopic arm conveyor belt, and is used to deliver the goods for palletizing to the telescopic arm conveyor belt.

[0018] In step S1, constraints are restricted and optimized based on dynamic constraints, and the Lagrangian modeling method is used to model the robot, specifically:

[0019] Given the parameters of each component of the robot, a kinematic model is built for it. The basis of kinematic modeling is the DH coordinate system. The robot arm is regarded as a combination of multiple connecting rods and multiple joints on the base. The origin of each coordinate of the DH coordinate system is set at each joint. Each joint includes the rotating part of the rotating frame, the rotating hinge part of the telescopic arm assembly, and the rotating hinge part of the end effector.

[0020] The relationship between each two joints is expressed by the connecting rod length ai and connecting rod torsion angle α i To describe the relationship between the connecting rods, the offset d i The angle θ between the normal lines of the two connecting rods i To describe, according to Figure 1 Determine the coordinate system of each connecting rod based on the model and parameters;

[0021] After establishing the link coordinate system, follow the steps below to achieve the conversion between two adjacent links:

[0022] Step A1: Wind around Z i-1 Axis rotation θ i Angle, so that x i-1 Axis goes to x i Same plane;

[0023] Step A2: Along Z i-1 Axis translation distance d i , so that x i-1 With x i in the same straight line;

[0024] Step A3: Along x I Axis translation distance a i , so that the origins of the two coordinate systems coincide;

[0025] Step A4: Wrap around x i-1 Axis rotation α i , so that Z i-1 Go to with Z i The same straight line; after transforming the two connecting rods, the overall DH coordinate system is obtained as follows Figure 3 shown.

[0026] Step A5: Kinematic modeling to obtain basic motion constraints, the constraints of each joint are as follows: Figure 3 shown.

[0027] The dynamic model of the robot is described by the formula

[0028]

[0029] τ represents the joint torque, are the velocity and acceleration of each joint of the robot, M(q) is the positive definite mass matrix, represents the centrifugal force and Coriolis force matrix, G(q) is the gravity direction, τ ex It is the additional torque caused by the conveyor belt shaft load and friction.

[0030] In step S2, in the dynamic model of the robot, the problem of the working efficiency of the robot arm is considered, and the objective function is established with the minimum time as the goal, and the energy consumption is used as the evaluation item of the objective function. Considering the motion constraints of the robot arm during the movement process, the constraints of the spatial motion of the robot arm are determined by the angular velocity and acceleration information of each joint; at the same time, the torque of each joint should also meet the constraints of the dynamic analysis; that is: taking the minimum time as the objective function, the energy consumption evaluation item is considered in the objective function at the same time, and the energy consumption required by the robot arm during the movement process is defined by the integral of the square of the joint torque over time. The objective function is defined as:

[0031] min T+μE;

[0032]

[0033] Where T is the time required for the robot to move, μ is the weighting coefficient, E is the total energy consumption, f is the friction between the conveyor belt and the load, v is the constant running speed of the conveyor belt, P is the constant power of the conveyor belt, τ i (t) is the torque of each joint.

[0034] In the dynamic model of the robot, its own constraints need to be considered during movement. The angular velocity and angular acceleration of each joint must meet the maximum constraint conditions. At the same time, the torque of each joint should also meet the maximum torque constraint. To ensure a smooth transition during the movement, the angular velocity at the starting point and the end point of the movement is zero. The above constraints are added to the model, and we have

[0035] is the joint velocity at any time;

[0036] is the joint acceleration at any time;

[0037] Set the start and end speed and acceleration values ​​to 0;

[0038] τ jmin ≤τ j ≤τ jmax , τ j is the joint torque at any time, where j = 1, 2, …, N.

[0039] In the dynamic model of the robot, when the packing path p(t) of the manipulator in the workspace is known, the corresponding path q(t) in the joint space is obtained according to the inverse kinematics analysis of the robot. Specifically, a monotonically increasing path parameter s(t) is introduced, where s is the displacement, which is related to the manipulator path and represents a one-dimensional path coordinate system;

[0040] The predetermined trajectory starts at t = 0 and ends at t = T, so that s(t) satisfies s(0) = 0 ≤ s(t) ≤ 1 = s(T), and T represents the time required for the trajectory to run;

[0041] Then the dynamic equation about s is:

[0042]

[0043] in

[0044]

[0045] s、 Substitute into the objective function and rewrite the time term as

[0046]

[0047] The energy required by the jth joint of the robot during operation is expressed as the integral of the square of the joint torque over time:

[0048]

[0049] The time term and the energy term are normalized and constructed as a joint optimization objective function, namely:

[0050]

[0051] Where μ is the trade-off coefficient of the energy term, and j represents the number of joints of the robot.

[0052] In step S3, the packing path of the robot arm is known, and the corresponding path in the joint space is obtained according to the motion analysis, and the path is introduced into the parameters so as to solve the subsequent optimization problem; the path parameters are set to ensure that the start and end conditions are met, and the time minimization requirements and energy consumption minimization requirements established previously are unified for planning and processing, and the two are constructed as a joint optimization function; the optimization problem is converted into a convex optimization form through variable substitution, and new optimization variables are introduced to convert the torque constraint into a linear constraint so that the objective function meets the requirements, and the convex relaxation method is used to further ensure that the global optimal solution is obtained; a complete optimization model is obtained including multiple constraints of speed, acceleration, torque, and torque change rate to ensure the stability of the trajectory under all constraints;

[0053] The problem model is transformed into a convex optimization form, and the global optimal solution is obtained according to the characteristics of convex optimization; specifically:

[0054] Introducing optimization variables and And meet From this, we can deduce that the velocity constraint of the jth joint of the robot arm is expressed as:

[0055]

[0056] It is deduced that the constraints are satisfied: in Represents the upper bound of b(s) and satisfies the conditions:

[0057]

[0058] Substituting the optimization variables into the optimization function, we can obtain the optimization control problem about a(s) and b(s):

[0059]

[0060] τ(s)=m(s)a(s)+c(s)b(s)+g(s);

[0061]

[0062] b(s)≥0;

[0063] b(0)=b(1)=0

[0064] Discretize the parameter s in the interval [0, 1] and use the SOCP algorithm to perform numerical solution to obtain the numerical solution. The basic steps are as follows:

[0065] First, the path pseudo parameter s∈[0,1] is equally divided into N intervals, with a total of N+1 discrete points, including the starting point and the end point;

[0066] Use s i represents the i-th discrete point, where i = 0, 1, ..., N; s0 = 0, s N =1

[0067] The total length of the interval is: Convert the pseudo velocity b(s) and pseudo acceleration a(s) of the path into variables b at discrete points i and a i ;

[0068] where b i = b(s i ) and a i =a(s i ), satisfying the relationship:

[0069] b i+1 =b i +a i αs;

[0070] For each s i , variable b irepresents the square of pseudo velocity at discrete points, variable a i represents the pseudo acceleration of the point;

[0071] Joint velocities and accelerations are also calculated at discrete points s i The and

[0072]

[0073] Among them, q′(si) and q″(si) are the trajectory q(s) at s i The first and second order derivatives at the points can control the movement speed and smoothness of the robot by constraining the joint velocity and acceleration at each discrete point;

[0074] Time Item is discretized into: The original continuous time target becomes the sum at each discrete point, and the time discretization is achieved through integral approximation;

[0075] The energy target term E can be expressed as follows after discretization:

[0076] where τ j，i represents the torque of the jth joint at the i-th discrete point. Through this discrete representation, the energy consumption is allocated to each discrete point;

[0077] Discretize the constraints such as path pseudo-velocity, pseudo-acceleration, torque and velocity and apply them to each discrete point, specifically:

[0078] Pseudo-speed constraint, satisfying the pseudo-speed upper limit constraint b at each discrete point i ≤ b max ;

[0079] Relationship between pseudo velocity and pseudo acceleration, b i+1 =b i +a i Δs;

[0080] Joint velocity constraints, discrete joint velocities satisfy

[0081] Joint acceleration constraints,

[0082] The moment constraint satisfies τ at each discrete point j，i ≤τ max ;

[0083] The final optimization objective function after discretization is defined at the discrete point as:

[0084]

[0085] To solve the non-convexity of the optimization problem, we first introduce a new optimization variable d i and e i And the nonlinear terms are replaced with variables to turn the target problem into a convex optimization problem, thereby ensuring the global optimal solution. The introduced variables satisfy the following constraints: After introducing new variables, the objective function is transformed into: After optimizing the objective function of the complete body, the YALMIP toolbox of MATLAB is used to solve it, and the solver called is CPLEX;

[0086] After the solution is completed, the solved discrete points are the path points of the corresponding trajectory.

[0087] After solving the path points, the optimal time T of the path can be obtained through t(s):

[0088]

[0089] When solving with MATLAB's YALMIP toolbox, the specific method is:

[0090] The effectiveness of the method is verified by using the robot toolbox in MATLAB. First, the code is written in MATLAB to input the structural parameters of the robot arm and generate a kinematic model. The kinematic structure model of the palletizing robot used in this example is as follows: Figure 4 As shown in the figure, the discrete space points are obtained by solving the previous steps, and the five-order B-spline lines are used to interpolate the discrete points in the space to finally obtain the optimized path; the speed and acceleration of the start and end are set to 0, so that the speed and acceleration of the trajectory are continuous, thereby ensuring the stability of the robot operation process;

[0091] The motion trajectory of the palletizing robot is as follows: Figure 6 As shown, the curve at the end changes as Figure 7 As shown in the figure, the velocity and acceleration curves of each joint are as follows: Figure 8 shown.

[0092] In step S4, the robot's working scenario is the cargo loading and unloading process. There are multiple targets and multiple paths in the packing process. The time, energy consumption and impact optimization of the robot's movement process are considered in the objective function, and all kinematic constraints are added to the optimization model to establish an optimization problem model for multi-target point trajectory optimization; the weight coefficients of various parameters are set, and the multi-target optimization model is solved when the predetermined path and constraint conditions remain unchanged;

[0093] The method of motion planning process is as follows:

[0094] The process of multi-target planning is regarded as a repeated planning movement of a single plan. In the process of single motion planning, the speed and acceleration of the start and end points are set to 0 to ensure the smooth operation of the robot. If the speed and acceleration of a single start and end point are 0 in the continuous movement of multiple targets, resulting in a sudden change in the speed and acceleration of the starting point of the movement, then the restriction of torque change is added; that is, in the entire model constraint, the constraint of the torque change rate is added to reduce the vibration of the start and stop of a single target point. The objective function of the torque change rate constraint is:

[0095]

[0096] Among them, Δτ i represents the torque change of joint j at adjacent discrete points, Δs is the increment of path parameters, and the constraint of torque change rate is added to the target optimization function model. When the rest of the path and constraint conditions remain unchanged, the trajectory points of multiple targets are solved and their trajectories are fitted. The simulation steps of a single target point are repeated to verify in MATLAB.

[0097] The present invention has the following beneficial effects:

[0098] 1. The present invention proposes a motion trajectory optimization method for a palletizing robot with time and energy as optimization targets under the condition of dynamic constraints, realizes the kinematic and dynamic simulation analysis of the palletizing robot, and achieves good performance optimization effect. The time-energy optimal trajectory can effectively reduce the time and energy consumption of the energy optimal trajectory.

[0099] 2. In the present invention, the time-energy optimal trajectory running time is significantly reduced without considering the impact constraint, but the required energy consumption is greatly increased and the jitter effect is greater. In terms of impact constraints, adding acceleration and torque change rate can effectively reduce the occurrence of jitter and make the operation more stable.

[0100] 3. After adding the constrained acceleration, the present invention can effectively reduce the occurrence of jitter, but the time is increased; applying the torque change rate constraint, the running time is shorter, but the energy consumption is increased, and the jitter optimization effect is not as good as the trajectory optimization effect of the constrained acceleration. BRIEF DESCRIPTION OF THE DRAWINGS

[0101] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0102] Attached Figure 1 is a schematic diagram of the three-dimensional structure of a palletizing robot in an embodiment of the present invention;

[0103] Attached Figure 2 is a physical schematic diagram of a palletizing robot in an embodiment of the present invention;

[0104] Attached Figure 3 Schematic diagram of the DH coordinate system and motion constraints of the palletizing robot in an embodiment of the present invention;

[0105] Attached Figure 4 It is a schematic diagram of the motion simulation of the MATLAB robot toolbox for the palletizing robot in an embodiment of the present invention;

[0106] Attached Figure 5 It is a basic flow chart of solving the optimization problem in the embodiment of the present invention;

[0107] Attached Figure 6 is a schematic diagram of trajectory simulation in an embodiment of the present invention;

[0108] Attached Figure 7 is a schematic diagram of a terminal simulation speed curve in an embodiment of the present invention;

[0109] Attached Figure 8 is a schematic diagram of the change curves of joint speed, acceleration and torque in an embodiment of the present invention;

[0110] In the figure: 1 is the robot chassis, 2 is the chassis frame, 3 is the electric control cabinet, 4 is the counterweights symmetrically distributed on both sides of the chassis, 5 is the primary conveyor belt, 6 is the rotating frame, 7 is the telescopic arm lifting servo cylinder, 8 is the telescopic arm assembly, and 9 is the end effector. DETAILED DESCRIPTION

[0111] As shown in the figure, Figure 1 is an example diagram of a robotic arm provided according to an embodiment of the present disclosure. Figure 1 As shown, the robot arm includes but is not limited to a base, multiple connecting rods, and multiple joints. 1 is the robot chassis, 2 is the chassis frame, 3 is the electric control cabinet, 4 is the counterweights symmetrically distributed on both sides of the chassis, 5 is the primary conveyor belt, 6 is the rotating frame, 7 is the telescopic arm lifting servo cylinder, 8 is the telescopic arm assembly, and 9 is the end effector. 1, 2, 3, 4, and 5 constitute the robot mobile chassis part, and 6, 7, and 8 constitute the robot arm part of the palletizing robot.

[0112] In accordance with Figure 1 In the case of the structural diagram, given the parameters of each component of the robot, a kinematic model is built for it. The basis of kinematic modeling is the DH coordinate system. For the robotic arm, the relationship between each two joints can be expressed by the connecting rod length a. i and connecting rod torsion angle α i To describe the relationship between the connecting rods, the offset d i The angle θ between the normal lines of the two connecting rods i To describe, according to Figure 1 The coordinate system of each connecting rod is determined by the model and parameters.

[0113] This example proposes a trajectory optimization method for a palletizing robot based on dynamic constraints. The palletizing robot can perform multi-objective box packing operations. Its mechanical arm includes a rotating frame (6) arranged at a base and capable of rotating horizontally. The rotating frame is provided with a telescopic arm assembly (8) driven by a telescopic arm lifting servo electric cylinder (7) to rotate vertically. The telescopic arm assembly is provided with a telescopic arm conveyor belt for conveying goods, and the end of the telescopic arm conveyor belt is provided with an end actuator (9) capable of rotating vertically.

[0114] The optimization method comprises the following steps:

[0115] Step S1, establishing a kinematic model and a dynamic model according to the structural parameters of the robot arm to obtain constraint conditions;

[0116] Step S2, combining kinematic and dynamic constraints, taking time and energy as optimization targets, and establishing an optimization function model;

[0117] Step S3: According to the optimization model, the optimization model is reconstructed and optimized by using a convex optimization method to solve the problem;

[0118] Step S4: according to the single-objective optimization problem, the torque change rate is added as a constraint condition, and the multi-objective function model is optimized in combination with the model to obtain the optimal trajectory of the multi-objective packing motion planning.

[0119] The base is a robot chassis (1) with a primary conveyor belt, and a chassis frame (2), an electric control cabinet (3), and counterweights (4) arranged on both sides of the chassis are provided on the chassis; the end of the primary conveyor belt is adjacent to the beginning of the telescopic arm conveyor belt, and is used to deliver the goods for palletizing to the telescopic arm conveyor belt.

[0120] In step S1, constraints are restricted and optimized based on dynamic constraints, and the Lagrangian modeling method is used to model the robot, specifically:

[0121] Given the parameters of each component of the robot, a kinematic model is built for it. The basis of kinematic modeling is the DH coordinate system. The robot arm is regarded as a combination of multiple connecting rods and multiple joints on the base. The origin of each coordinate of the DH coordinate system is set at each joint. Each joint includes the rotating part of the rotating frame, the rotating hinge part of the telescopic arm assembly, and the rotating hinge part of the end effector.

[0122] The relationship between each two joints is expressed by the connecting rod length a i and connecting rod torsion angle α i To describe the relationship between the connecting rods, the offset d i The angle θ between the normal lines of the two connecting rods i To describe, according to Figure 1 Determine the coordinate system of each connecting rod based on the model and parameters;

[0123] After establishing the link coordinate system, follow the steps below to achieve the conversion between two adjacent links:

[0124] Step A1: Wind around Z i-1 Axis rotation θ i Angle, so that x i-1 Axis goes to x i Same plane;

[0125] Step A2: Along Z i-1 Axis translation distance d i , so that x i-1 With x i in the same straight line;

[0126] Step A3: Along x I Axis translation distance a i , so that the origins of the two coordinate systems coincide;

[0127] Step A4: Wrap around x i-1 Axis rotation α i , so that Z i-1 Go to with Z i The same straight line; after transforming the two connecting rods, the overall DH coordinate system is obtained as follows Figure 3 shown.

[0128] Step A5: Kinematic modeling to obtain basic motion constraints, the constraints of each joint are as follows: Figure 3 shown.

[0129] The dynamic model of the robot is described by the formula

[0130]

[0131] τ represents the joint torque, are the velocity and acceleration of each joint of the robot, M(q) is the positive definite mass matrix, represents the centrifugal force and Coriolis force matrix, G(q) is the gravity direction, τ ex It is the additional torque caused by the conveyor belt shaft load and friction.

[0132] In step S2, in the dynamic model of the robot, the problem of the working efficiency of the robot arm is considered, and the objective function is established with the minimum time as the goal, and the energy consumption is used as the evaluation item of the objective function. Considering the motion constraints of the robot arm during the movement process, the constraints of the spatial motion of the robot arm are determined by the angular velocity and acceleration information of each joint; at the same time, the torque of each joint should also meet the constraints of the dynamic analysis; that is: taking the minimum time as the objective function, the energy consumption evaluation item is considered in the objective function at the same time, and the energy consumption required by the robot arm during the movement process is defined by the integral of the square of the joint torque over time. The objective function is defined as:

[0133] min T+μE;

[0134]

[0135] Where T is the time required for the robot to move, μ is the weighting coefficient, E is the total energy consumption, f is the friction between the conveyor belt and the load, v is the constant running speed of the conveyor belt, P is the constant power of the conveyor belt, τ i (t) is the torque of each joint.

[0136] In the dynamic model of the robot, its own constraints need to be considered during movement. The angular velocity and angular acceleration of each joint must meet the maximum constraint conditions. At the same time, the torque of each joint should also meet the maximum torque constraint. To ensure a smooth transition during the movement, the angular velocity at the starting point and the end point of the movement is zero. The above constraints are added to the model, and we have

[0137] is the joint velocity at any time;

[0138] is the joint acceleration at any time;

[0139] Set the start and end speed and acceleration values ​​to 0;

[0140] τ jmin ≤τ j ≤τ jmax , τ j is the joint torque at any time, where j = 1, 2, …, N.

[0141] In the dynamic model of the robot, when the packing path p(t) of the manipulator in the workspace is known, the corresponding path q(t) in the joint space is obtained according to the inverse kinematics analysis of the robot. Specifically, a monotonically increasing path parameter s(t) is introduced, where s is the displacement, which is related to the manipulator path and represents a one-dimensional path coordinate system;

[0142] The predetermined trajectory starts at t = 0 and ends at t = T, so that s(t) satisfies s(0) = 0 ≤ s(t) ≤ 1 = s(T), and T represents the time required for the trajectory to run;

[0143] Then the dynamic equation about s is:

[0144]

[0145] s、 Substitute into the objective function and rewrite the time term as

[0146]

[0147] The energy required by the jth joint of the robot during operation is expressed as the integral of the square of the joint torque over time:

[0148]

[0149] The time term and the energy term are normalized and constructed as a joint optimization objective function, namely:

[0150]

[0151] Where μ is the trade-off coefficient of the energy term, and j represents the number of joints of the robot.

[0152] In step S3, the packing path of the robot arm is known, and the corresponding path in the joint space is obtained according to the motion analysis, and the path is introduced into the parameters so as to solve the subsequent optimization problem; the path parameters are set to ensure that the start and end conditions are met, and the time minimization requirements and energy consumption minimization requirements established previously are unified for planning and processing, and the two are constructed as a joint optimization function; the optimization problem is converted into a convex optimization form through variable substitution, and new optimization variables are introduced to convert the torque constraint into a linear constraint so that the objective function meets the requirements, and the convex relaxation method is used to further ensure that the global optimal solution is obtained; a complete optimization model is obtained including multiple constraints of speed, acceleration, torque, and torque change rate to ensure the stability of the trajectory under all constraints;

[0153] The problem model is transformed into a convex optimization form, and the global optimal solution is obtained according to the characteristics of convex optimization; specifically:

[0154] Introducing optimization variables and And meet From this, we can deduce that the velocity constraint of the jth joint of the robot arm is expressed as:

[0155]

[0156] It is deduced that the constraints are satisfied: in Represents the upper bound of b(s) and satisfies the conditions:

[0157]

[0158] Substituting the optimization variables into the optimization function, we can obtain the optimization control problem about a(s) and b(s):

[0159]

[0160] τ(s)=m(s)a(s)+c(s)b(s)+g(s);

[0161]

[0162] b(s)≥0;

[0163] b(0)=b(1)=0

[0164] Discretize the parameter s in the interval [0, 1] and use the SOCP algorithm to perform numerical solution to obtain the numerical solution. The basic steps are as follows:

[0165] First, the path pseudo parameter s∈[0,1] is equally divided into N intervals, with a total of N+1 discrete points, including the starting point and the end point;

[0166] Use s i represents the i-th discrete point, where i = 0, 1, ..., N; s0 = 0, s N =1

[0167] The total length of the interval is:

[0168] Convert the pseudo velocity b(s) and pseudo acceleration a(s) of the path into variables b at discrete points i and a i ;

[0169] where b i = b(s i ) and a i =a(s i ), satisfying the relationship:

[0170] b i+1 =b i +a i αs;

[0171] For each s i , variable b i represents the square of pseudo velocity at discrete points, variable a i represents the pseudo acceleration of the point;

[0172] Joint velocities and accelerations are also calculated at discrete points s i The and

[0173]

[0174] Among them, q′(si) and q″(si) are the trajectory q(s) at s i The first and second order derivatives at the points can control the movement speed and smoothness of the robot by constraining the joint velocity and acceleration at each discrete point;

[0175] Time Item is discretized into:

[0176] The original continuous time target becomes the sum at each discrete point, and the time discretization is achieved through integral approximation;

[0177] The energy target term E can be expressed as follows after discretization:

[0178] where τ j，i represents the torque of the jth joint at the i-th discrete point. Through this discrete representation, the energy consumption is allocated to each discrete point;

[0179] Discretize the constraints such as path pseudo-velocity, pseudo-acceleration, torque and velocity and apply them to each discrete point, specifically:

[0180] Pseudo-speed constraint, satisfying the pseudo-speed upper limit constraint b at each discrete point i ≤ b max ;

[0181] Relationship between pseudo velocity and pseudo acceleration, b i+1 =b i +a i Δs;

[0182] Joint velocity constraints, discrete joint velocities satisfy

[0183] Joint acceleration constraints,

[0184] The moment constraint satisfies τ at each discrete point j，i ≤τ max ;

[0185] The final optimization objective function after discretization is defined at the discrete point as:

[0186]

[0187] To solve the non-convexity of the optimization problem, we first introduce a new optimization variable d i and e i And the nonlinear terms are replaced with variables to turn the target problem into a convex optimization problem, thereby ensuring the global optimal solution. The introduced variables satisfy the following constraints:

[0188] After introducing new variables, the objective function is transformed into:

[0189] After optimizing the objective function of the complete body, the YALMIP toolbox of MATLAB is used to solve it, and the solver called is CPLEX;

[0190] After the solution is completed, the solved discrete points are the path points of the corresponding trajectory.

[0191] After solving the path points, the optimal time T of the path can be obtained through t(s):

[0192]

[0193] When solving with MATLAB's YALMIP toolbox, the specific method is:

[0194] The effectiveness of the method is verified by using the robot toolbox in MATLAB. First, the code is written in MATLAB to input the structural parameters of the robot arm and generate a kinematic model. The kinematic structure model of the palletizing robot used in this example is as follows: Figure 4 As shown in the figure, the discrete space points are obtained by solving the previous steps, and the five-order B-spline lines are used to interpolate the discrete points in the space to finally obtain the optimized path; the speed and acceleration of the start and end are set to 0, so that the speed and acceleration of the trajectory are continuous, thereby ensuring the stability of the robot operation process;

[0195] The motion trajectory of the palletizing robot is as follows: Figure 6 As shown, the curve at the end changes as Figure 7 As shown in the figure, the velocity and acceleration curves of each joint are as follows: Figure 8 shown.

[0196] In step S4, the robot's working scenario is the cargo loading and unloading process. There are multiple targets and multiple paths in the packing process. The time, energy consumption and impact optimization of the robot's movement process are considered in the objective function, and all kinematic constraints are added to the optimization model to establish an optimization problem model for multi-target point trajectory optimization; the weight coefficients of various parameters are set, and the multi-target optimization model is solved when the predetermined path and constraint conditions remain unchanged;

[0197] The method of motion planning process is as follows:

[0198] The process of multi-target planning is regarded as a repeated planning movement of a single plan. In the process of single motion planning, the speed and acceleration of the start and end points are set to 0 to ensure the smooth operation of the robot. If the speed and acceleration of a single start and end point are 0 in the continuous movement of multiple targets, resulting in a sudden change in the speed and acceleration of the starting point of the movement, then the restriction of torque change is added; that is, in the entire model constraint, the constraint of the torque change rate is added to reduce the vibration of the start and stop of a single target point. The objective function of the torque change rate constraint is:

[0199]

[0200] Among them, Δτ jrepresents the torque change of joint j at adjacent discrete points, Δs is the increment of path parameters, and the constraint of torque change rate is added to the target optimization function model. When the rest of the path and constraint conditions remain unchanged, the trajectory points of multiple targets are solved and their trajectories are fitted. The simulation steps of a single target point are repeated to verify in MATLAB.

Claims

1. A palletizing robot trajectory optimization method based on dynamic constraints, characterized in that: The palletizing robot can perform multi-target boxing operations, and its mechanical arm includes a rotating frame (6) arranged at a base and capable of rotating horizontally; a telescopic arm assembly (8) driven by a telescopic arm lifting servo electric cylinder (7) to rotate vertically is arranged at the rotating frame; a telescopic arm conveyor belt for conveying goods is arranged at the telescopic arm conveyor belt, and an end actuator (9) capable of rotating vertically is arranged at the end of the telescopic arm conveyor belt; The optimization method comprises the following steps: Step S1, establishing a kinematic model and a dynamic model according to the structural parameters of the robot arm to obtain constraint conditions; Step S2, combining kinematic and dynamic constraints, taking time and energy as optimization targets, and establishing an optimization function model; Step S3: According to the optimization model, the optimization model is reconstructed and optimized by using a convex optimization method to solve the problem; Step S4: according to the single-objective optimization problem, the torque change rate is added as a constraint condition, and the multi-objective function model is optimized in combination with the model to obtain the optimal trajectory of the multi-objective packing motion planning.

2. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 1, characterized in that: The base is a robot chassis (1) with a primary conveyor belt, and a chassis frame (2), an electric control cabinet (3), and counterweights (4) arranged on both sides of the chassis are provided on the chassis; the end of the primary conveyor belt is adjacent to the beginning of the telescopic arm conveyor belt, and is used to deliver the goods for palletizing to the telescopic arm conveyor belt.

3. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 1, characterized in that: In step S1, constraints are restricted and optimized based on dynamic constraints, and the Lagrangian modeling method is used to model the robot, specifically: Given the parameters of each component of the robot, a kinematic model is built for it. The basis of kinematic modeling is the DH coordinate system, and the robot arm is regarded as a combination of multiple links and multiple joints on the base; The relationship between each two joints is expressed by the connecting rod length a i and connecting rod torsion angle α i Describe the relationship between the connecting rods using the offset d i The angle θ between the normal lines of the two connecting rods i describe; After establishing the link coordinate system, follow the steps below to achieve the conversion between two adjacent links: Step A1: Wind around Z i-1 Axis rotation θ i Angle, so that x i-1 Axis goes to x i Same plane; Step A2: Along Z i-1 Axis translation distance d i , so that x i-1 With x i in the same straight line; Step A3: Along x I Axis translation distance a i , so that the origins of the two coordinate systems coincide; Step A4: Wrap around x i-1 Axis rotation α i , so that Z i-1 Go to with Z i The same straight line. Step A5: Kinematic modeling to obtain basic motion constraints.

4. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 3 is characterized in that: The dynamic model of the robot is described by the formula τ represents the joint torque, are the velocity and acceleration of each joint of the robot, M(q) is the positive definite mass matrix, represents the centrifugal force and Coriolis force matrix, G(q) is the gravity direction, τ ex It is the additional torque caused by the conveyor belt shaft load and friction.

5. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 3 is characterized in that: In step S2, in the dynamic model of the robot, the problem of the working efficiency of the robot arm is considered, and the objective function is established with the minimum time as the goal, and the energy consumption is used as the evaluation item of the objective function. Considering the motion constraints of the robot arm during the movement process, the constraints of the spatial motion of the robot arm are determined by the angular velocity and acceleration information of each joint; at the same time, the torque of each joint should also meet the constraints of the dynamic analysis; that is: taking the minimum time as the objective function, the energy consumption evaluation item is considered in the objective function at the same time, and the energy consumption required by the robot arm during the movement process is defined by the integral of the square of the joint torque over time. The objective function is defined as: min T+μE; Where T is the time required for the robot to move, μ is the weighting coefficient, E is the total energy consumption, f is the friction between the conveyor belt and the load, v is the constant running speed of the conveyor belt, P is the constant power of the conveyor belt, τ i (t) is the torque of each joint.

6. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 5 is characterized in that: In the dynamic model of the robot, its own constraints need to be considered during movement. The angular velocity and angular acceleration of each joint must meet the maximum constraint conditions. At the same time, the torque of each joint should also meet the maximum torque constraint. To ensure a smooth transition during the movement, the angular velocity at the starting point and the end point of the movement is zero. The above constraints are added to the model, and we have is the joint velocity at any time; is the joint acceleration at any time; Set the start and end speed and acceleration values ​​to 0; τ jmin ≤τ j ≤τ jmax , τ j is the joint torque at any time, where j = 1, 2, …, N.

7. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 6, characterized in that: In the dynamic model of the robot, when the packing path p(t) of the manipulator in the workspace is known, the corresponding path q(t) in the joint space is obtained according to the inverse kinematics analysis of the robot. Specifically, a monotonically increasing path parameter s(t) is introduced, where s is the displacement, which is related to the manipulator path and represents a one-dimensional path coordinate system; The predetermined trajectory starts at t = 0 and ends at t = T, so that s(t) satisfies s(0) = 0 ≤ s(t) ≤ 1 = s(T), and T represents the time required for the trajectory to run; Then the dynamic equation about s is: in s、 Substitute into the objective function and rewrite the time term as The energy required by the jth joint of the robot during operation is expressed as the integral of the square of the joint torque over time: The time term and the energy term are normalized and constructed as a joint optimization objective function, namely: Where μ is the trade-off coefficient of the energy term, and j represents the number of joints of the robot.

8. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 7, characterized in that: In step S3, the packing path of the robot arm is known, and the corresponding path in the joint space is obtained according to the motion analysis, and the path is introduced into the parameters so as to solve the subsequent optimization problem; Set the path parameters to ensure that the start and end conditions are met, unify the previously established time minimization requirements and energy consumption minimization requirements, and construct the two into a joint optimization function; The optimization problem is transformed into a convex optimization form through variable substitution, and new optimization variables are introduced to transform the moment constraint into a linear constraint so that the objective function meets the requirements. The convex relaxation method is used to further ensure that the global optimal solution is obtained. The complete optimization model includes multiple constraints of speed, acceleration, torque, and torque change rate to ensure the stability of the trajectory under all constraints; The problem model is transformed into a convex optimization form, and the global optimal solution is obtained according to the characteristics of convex optimization; specifically: Introducing optimization variables and And meet From this, we can deduce that the velocity constraint of the jth joint of the robot arm is expressed as: It is deduced that the constraints are satisfied: in represents the upper bound of b(s) and satisfies the conditions: Substituting the optimization variables into the optimization function, we can obtain the optimization control problem about a(s) and b(s): τ(s)=m(s)a(s)+c(s)b(s)+g(s); b(s)≥0; b(0)=b(1)=0; Discretize the parameter s in the interval [0,1] and use the SOCP algorithm to solve numerically to obtain the numerical solution. The basic steps are as follows: First, divide the path pseudo parameter s∈[0,1] into N intervals with equal intervals, with a total of N+1 discrete points, including the starting point and the end point; Use s i represents the i-th discrete point, where i=0,1,…,N; s0=0,s N =1 The total length of the interval is: Convert the pseudo velocity b(s) and pseudo acceleration a(s) of the path into variables b at discrete points i and a i ; where b i =b(s i ) and a i =a(s i ), satisfying the relationship: b i+1 =b i +a i Δs; For each s i , variable b i represents the square of pseudo velocity at discrete points, variable a i represents the pseudo acceleration of the point; Joint velocities and accelerations are also calculated at discrete points s i The and Among them, q′(si) and q″(si) are the trajectory q(s) at s i The first and second order derivatives at the points can control the movement speed and smoothness of the robot by constraining the joint velocity and acceleration at each discrete point; Time Item is discretized into: The original continuous time target becomes the sum at each discrete point, and the time discretization is achieved through integral approximation; The energy target term E can be expressed as follows after discretization: where τ j，i represents the torque of the jth joint at the i-th discrete point. Through this discrete representation, the energy consumption is allocated to each discrete point; Discretize the constraints such as path pseudo-velocity, pseudo-acceleration, torque and velocity and apply them to each discrete point, specifically: Pseudo-speed constraint, satisfying the pseudo-speed upper limit constraint b at each discrete point i ≤ b max ; Relationship between pseudo velocity and pseudo acceleration, b i+1 =b i +a i Δs; Joint velocity constraints, discrete joint velocities satisfy Joint acceleration constraints, The moment constraint satisfies τ at each discrete point j，i ≤τ max ; The final optimization objective function after discretization is defined at the discrete point as: To solve the non-convexity of the optimization problem, we first introduce a new optimization variable d i and e i And the nonlinear terms are replaced with variables to make the target problem a convex optimization problem, thereby ensuring the global optimal solution. The introduced variables satisfy the following constraints: After introducing new variables, the objective function is transformed into: After optimizing the objective function of the complete body, the YALMIP toolbox of MATLAB is used to solve it, and the solver called is CPLEX; After the solution is completed, the solved discrete points are the path points of the corresponding trajectory. After solving the path points, the optimal time T of the path can be obtained through t(s):

9. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 8, characterized in that: When solving with MATLAB's YALMIP toolbox, the specific method is: The effectiveness of the method was verified using the Robot Toolbox in MATLAB. First, the code was written in MATLAB to input the structural parameters of the robotic arm and generate a kinematic model. The points in the discrete space were solved by the previous steps, and the discrete points in the space were interpolated using quintic B-spline lines to finally obtain the optimized path. The start and end speeds and accelerations were set to 0 to make the speed and acceleration of the trajectory continuous, thereby ensuring the smoothness of the robotic arm's operation.

10. The method for optimizing the trajectory of a palletizing robot based on dynamic constraints according to claim 8, characterized in that: In step S4, the robot's working scenario is the cargo loading and unloading process. There are multiple targets and multiple paths in the packing process. The time, energy consumption and impact optimization of the robot's movement process are considered in the objective function, and all kinematic constraints are added to the optimization model to establish an optimization problem model for multi-target point trajectory optimization; the weight coefficients of various parameters are set, and the multi-target optimization model is solved when the predetermined path and constraint conditions remain unchanged; The method of motion planning process is as follows: The process of multi-target planning is regarded as a repeated planning movement of a single plan. In the process of single motion planning, the speed and acceleration of the start and end points are set to 0 to ensure the smooth operation of the robot. If the speed and acceleration of a single start and end point are 0 in the continuous movement of multiple targets, resulting in a sudden change in the speed and acceleration of the starting point of the movement, then the restriction of torque change is added; that is, in the entire model constraint, the constraint of the torque change rate is added to reduce the vibration of the start and stop of a single target point. The objective function of the torque change rate constraint is: Among them, Δτ j represents the torque change of joint j at adjacent discrete points, Δs is the increment of path parameters, and the constraint of torque change rate is added to the target optimization function model. When the rest of the path and constraint conditions remain unchanged, the trajectory points of multiple targets are solved and their trajectories are fitted, and the simulation steps of a single target point are repeated to verify in MATLAB.

Images