Path planning method and system for intelligent loading and stacking robot with body based on pose decomposition
Through pose decomposition and particle swarm algorithm optimization, combined with B-spline curve, the path rigidity and local optimization problems of traditional palletizing systems are solved, and efficient and smooth path planning of intelligent loading and palletizing robots is realized to meet the needs of different vehicle models and packaging specifications.
Patent Information
- Application Number
- CN202510917900.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-03
- Publication Date
- 2025-08-29
AI Technical Summary
The path planning of traditional palletizing systems is rigid and cannot dynamically respond to changes in vehicle length and package shape, resulting in a lot of empty strokes, low efficiency, insufficient coordinated control, high debugging costs, and particle swarm optimization algorithms are prone to fall into local optimization, and the path smoothness optimization effect is unstable.
The path planning method of embodied intelligent loading and palletizing robot based on pose decomposition is adopted. By obtaining the information of the carrier vehicle and cargo, the particle swarm algorithm is used to optimize the local path of each joint, and the particle velocity and position are updated in combination with the global optimal, historical optimal and dimensional optimal positions. The trajectory is optimized using the B-spline curve to achieve the smoothness and efficiency of the path.
The efficiency of loading and palletizing robots and the adaptability of path planning are improved, and the problems of non-optimal path length and many turning points in traditional methods are solved, so as to realize efficient and smooth path planning of robots in complex environments.
Smart Images
Figure CN120552070A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of automation control technology, and specifically relates to a path planning method and system for an embodied intelligent loading and stacking robot based on posture decomposition. Background Art
[0002] In logistics, warehousing, and smart manufacturing scenarios, palletizing robots must adapt to the demands of varying vehicle sizes, packaging specifications, and stacking rules. Traditional palletizing systems suffer from the following issues: 1. Rigid path planning: Fixed programming models cannot dynamically respond to changes in vehicle length and package size, resulting in frequent empty trips and low efficiency. 2. Inadequate collaborative control: Multi-cargo task allocation relies on manual scheduling, which can easily lead to path conflicts and waiting delays. 3. Poor adaptability: Irregular-shaped packages or complex stacking rules (such as zigzag arrangements) require manual parameter reconfiguration, resulting in high debugging costs. Furthermore, traditional palletizing robot path planning primarily uses traditional algorithms such as A* and RRT to generate initial paths, but these algorithms suffer from non-optimal path lengths and numerous turning points. Particle swarm optimization (PSO) has been introduced into the path optimization process due to its parallel search capabilities. PSO uses a velocity-position update formula to iteratively correct path node coordinates. However, existing PSO methods rely solely on the historical optimal solution of the individual or group, making them prone to local optima in complex obstacle environments. Furthermore, the optimized path suffers from unstable path smoothness optimization. Summary of the Invention
[0003] The purpose of the present invention is to provide a path planning method and system for an embodied intelligent loading and stacking robot based on posture decomposition.
[0004] In a first aspect, the present invention provides a path planning method for an embodied intelligent loading and stacking robot based on posture decomposition, the method comprising:
[0005] Obtain the transport vehicle information and the loaded cargo information, and set the placement of the loaded cargo in the carriage of the transport vehicle based on the transport vehicle information and the loaded cargo information; obtain the initial point and target point of each loaded cargo according to the placement position of the loaded cargo; obtain the global path of the end of the palletizing robot arm on the palletizing robot from the initial point and the target point through the path planning algorithm; obtain the initial local path of each joint on the palletizing robot arm according to the global path, and use the particle swarm algorithm to optimize the time required between adjacent path nodes in the initial local path corresponding to each joint to obtain the final local path; in the particle swarm algorithm, update the particle speed and position by setting the global optimal position, the historical optimal position and the dimensional optimal position; the palletizing robot performs palletizing according to the final local path.
[0006] Preferably, the method for obtaining the optimal position of the dimension is as follows:
[0007] Get the adjacent particles of each particle; select the optimal individual of each dimension from all the adjacent particles of the particle to form the dimension update position; the optimal individual of each dimension in the dimension update position The method to obtain is as follows:
[0008]
[0009]
[0010] in, is the fitness function; For particles No. adjacent particles; For particles No. The neighboring particles Dimensional individual; For particles No. dimensional particle position; is the optimal position of the dimension; Represents particles All neighboring particles of Indicates finding the position of the corresponding particle; D is the particle dimension;
[0011] Update the dimension optimal position based on the dimension update position.
[0012] Preferably, the method for updating the dimension optimal position based on the dimension update position is as follows: respectively obtain the fitness corresponding to the dimension update position and the dimension optimal position; if the fitness corresponding to the dimension update position is greater than the fitness corresponding to the dimension optimal position, then use the dimension update position as the updated dimension optimal position; otherwise, the update fails; if the number of update failures is greater than a preset iteration number threshold, then randomly generate several particles, compare the randomly generated particles with the original particles for dominance, and randomly select one particle from the dominated particles to replace the original particle.
[0013] Preferably, the method for obtaining the adjacent particles is as follows: The particle position , if there exists a particle b that satisfies the condition , then particle b is the adjacent particle of particle a; where, is the maximum distance between particles; is the distance threshold.
[0014] Preferably, the particle velocity and position are updated as follows:
[0015] The global update speed, dimensional update speed and historical update speed of the particle in different dimensions are obtained according to the global optimal position, dimensional optimal position and historical optimal position respectively; the dimensions corresponding to different speed updates are mutually exclusive; the updated speed is composed based on the global update speed, dimensional update speed and historical update speed; the updated position of the particle is obtained based on the updated speed of the particle.
[0016] As a preferred method, the method for obtaining the final local path is as follows:
[0017] Initialize the particle swarm; set the fitness function and obtain the fitness of each particle based on the fitness function; select the global optimal position and the historical optimal position based on the fitness of the particle, and update the dimension optimal position; update the particle speed and position based on the global optimal position, the historical optimal position and the dimension optimal position; repeat the above process until the termination condition is reached, and take the particle corresponding to the historical optimal position as the final local path.
[0018] Preferably, the fitness function includes an optimization target and a penalty term; the optimization target is the time required for the path corresponding to the particle; and the penalty term is obtained based on the constraint equation of the joint.
[0019] Preferably, the method for obtaining the initial local path is as follows:
[0020] The palletizing robot arm is parametrically modeled, and the global path is discretized in Cartesian space to obtain N path nodes. Inverse kinematics calculations are performed on the nodes in Cartesian space to obtain the position sequence of each joint in the joint space. The position sequence of each joint is optimized using B-spline curves to obtain the initial local path.
[0021] Preferably, the transport vehicle information includes the type of carriage, the size of the vehicle body and the placement angle of the transport vehicle; and the loaded cargo information includes the size of the loaded cargo.
[0022] In a second aspect, the present invention provides an embodied intelligent loading and palletizing robot path planning system based on posture decomposition, which is used to execute the above-mentioned embodied intelligent loading and palletizing robot path planning method; the embodied intelligent loading and palletizing robot path planning system includes a palletizing robot and an environmental perception module, a controller, a wireless communication module, and a path generation module installed on the palletizing robot; the palletizing robot is provided with a palletizing robot arm including multiple joints; the environmental perception module includes a vision module and an ultrasonic obstacle avoidance device; the vision module is used to identify the information of the carrying vehicle and the loaded cargo information; the ultrasonic obstacle avoidance device is used to detect the position of obstacles and the palletizing robot arm; the controller is used to control the palletizing robot to perform palletizing; the path generation module includes a global path planning module, a local path planning module and a local path optimization module; the global path planning module is used to generate the path of the end of the palletizing robot arm; the local path planning module is used to generate the path of each joint on the palletizing robot arm; and the local path optimization module is used to optimize the path of each joint.
[0023] The present invention has the following beneficial effects:
[0024] 1. The present invention optimizes the initial local path corresponding to each joint on the palletizing robot arm through particle swarm optimization, and updates the particle speed and position according to the global optimal position, historical optimal position and dimension optimal position respectively. It solves the problem that the learning strategy of the traditional particle swarm algorithm causes particles to move away from the global optimal solution and fall into the local optimal solution, and improves the optimization effect of the particle swarm on the initial local path.
[0025] 2. The present invention decomposes the global path into the local path of each joint through posture decomposition, thereby performing path planning on the local path, thereby improving the efficiency of the loading and stacking robot in handling goods; at the same time, the present invention optimizes the local path of each joint through B-spline curves, thereby meeting the loading and stacking robot's requirement for smooth trajectory. BRIEF DESCRIPTION OF THE DRAWINGS
[0026] Figure 1 Schematic diagram of the path planning system for the embodied intelligent loading and palletizing robot in Example 1 of the present invention.
[0027] Figure 2 Schematic diagram of the structure of the palletizing robot in Example 1 of the present invention.
[0028] Figure 3 This is a flow chart of the path planning method for the embodied intelligent loading and palletizing robot in Example 2 of the present invention.
[0029] Figure 4 Schematic diagram of the method for obtaining the optimal position in the dimension in Example 2 of the present invention.
[0030] Figure 5 Schematic diagram of particle velocity update in Example 2 of the present invention. DETAILED DESCRIPTION
[0031] The present invention will be further described below with reference to the accompanying drawings.
[0032] Example 1
[0033] like Figure 1 and Figure 2 As shown, a path planning system for an embodied intelligent loading and palletizing robot based on pose decomposition includes a palletizing robot, a host computer, and an environmental perception module, controller, wireless communication module, and path generation module installed on the palletizing robot. The palletizing robot is equipped with a palletizing arm with five joints. The environmental perception module includes a vision module and an ultrasonic obstacle avoidance device; the vision module is used to identify vehicle information and loaded cargo information; the ultrasonic obstacle avoidance device is used to detect obstacles and the position of the palletizing arm. The controller is used to control the palletizing robot to perform palletizing. The path generation module includes a global path planning module, a local path planning module, and a local path optimization module; the global path planning module is used to generate a path for the end of the palletizing arm; the local path planning module is used to generate a path for each joint of the palletizing arm; and the local path optimization module is used to optimize the path for each joint. The host computer is used to provide visual operation, making it easier for users to adjust the placement of loaded cargo. In this embodiment, the vision module uses a 3D laser radar.
[0034] Example 2
[0035] like Figure 3 As shown, a path planning method for an embodied intelligent loading and stacking robot based on posture decomposition adopts the palletizing robot path planning system in Example 1; the path planning method for an embodied intelligent loading and stacking robot comprises the following steps:
[0036] Step 1: Use the vision module to inspect the cargo loading area. If a vehicle is present in the cargo loading area, the controller switches the palletizing robot from empty mode to ready mode. The vision module identifies the vehicle and loaded cargo information. Vehicle information includes the type of carriage, dimensions, and placement angle of the vehicle. Loaded cargo information includes the dimensions of the loaded cargo.
[0037] Step 2: Transmit the vehicle and cargo information to the host computer via the wireless communication module, and display the vehicle compartment and cargo in a graphical format on the host computer. The host computer sets the cargo stacking information, i.e., the placement of all cargo in the vehicle compartment, and obtains the initial and target points of each cargo based on the placement of the cargo.
[0038] Step 3: The initial point and target point of each loaded cargo are transmitted to the global path planning module through the controller, and the global path planning module uses the RRT (rapid traversal random tree) algorithm to generate the optimal path (global path) of the end of the stacking robot arm from the initial point and the target point.
[0039] Step 4: The global path is transmitted to the local path planning module through the controller to obtain the path of each joint on the robotic arm of the palletizing robot (initial local path). The specific process is as follows:
[0040] 4-1. Perform DH (Denavit-Hartenberg) parameter modeling on the palletizing robot arm, discretize the global path in Cartesian space, obtain N path nodes, perform inverse kinematics calculations on the nodes in Cartesian space, and obtain the position sequence of each joint in the joint space.
[0041] 4-2. Using quintic B-spline curves to interpolate the joint angle expressions of the palletizing robot, the curves for the continuous trajectory of each joint are obtained as follows:
[0042] (1)
[0043] in, is the change vector of the joint position; is the basis function of the k-degree B-spline curve; To control the vertex vector, ; To control the number of vertices; For the control node.
[0044] 4-3. Normalize the time nodes by cumulative time parameterization method and obtain the control nodes of the quintic B-spline curve , which is expressed as follows:
[0045] (2)
[0046] in, , is the normalized time series.
[0047] Construct the constraint equations of angular velocity, angular acceleration, torsion and torque of each joint, which are expressed as follows:
[0048] (3)
[0049] in, is the set of time required to move between adjacent path nodes (decision variables), ; ; is the time required from the starting path node to path node i; Respectively Angular velocity, angular acceleration, torque and moment of each joint; are the mth joint at time point Instantaneous angular velocity, instantaneous angular acceleration, instantaneous torque and instantaneous moment; are the maximum values of angular velocity, angular acceleration, torsion and torque of the mth joint respectively; ; M is the number of joints on the palletizing robot arm.
[0050] 4-4. Constructing optimization goals ; Based on optimization goals The trajectory curves of joint angular velocity and angular acceleration are set by the constraint equations of the joints, which are expressed as:
[0051] (4)
[0052] in, are the trajectory curves of joints, joint angular velocity and angular acceleration respectively; is the position of the i-th node in the joint space; To control the vertex vector; and are the joint starting angular velocity and joint starting angular acceleration respectively; and are the joint termination angular velocity and joint termination angular acceleration respectively.
[0053] 4-5. Based on the trajectory curves of the joints, joint angular velocities, and angular accelerations, obtain the path (initial local path) corresponding to each joint when the end of the palletizing robot arm moves from the starting point to the target point.
[0054] Step 5: The initial local paths of each joint on the palletizing robot arm are transmitted to the local path optimization module through the controller. The particle swarm algorithm is used to optimize the time required for each joint to move between adjacent path nodes of the initial local path, and the optimal local path corresponding to each joint is obtained. The method for obtaining the optimal local path corresponding to a single joint is as follows:
[0055] 5-1. Particle swarm initialization
[0056] Use decision variables as particles in the particle swarm; initialize the swarm size , particle position ,speed , particle dimension , inertia weight value , learning factor , global optimal position , the best historical position , optimal position of dimension , learning probability , Elite Probability , random guided learning continuous algebraic threshold and the maximum number of iterations .
[0057] 5-2. Fitness calculation
[0058] Construct the penalty term of the fitness function based on the constraint equation of the joint, and combine the penalty term and the optimization objective Constructing the fitness function in particle swarm optimization ; Get the fitness of each particle and select the global optimal position , the best historical position .
[0059] 5-3. Update the optimal position of the dimension
[0060] For particles The particle position , if there exists a particle b that satisfies the condition , then particle b is the adjacent particle of particle a; where, is the maximum distance between particles; is the distance threshold, ; is the current iteration number; is the maximum number of iterations. Figure 4 As shown, from the particle Select the best individual in each dimension from all adjacent particles to form the dimension and update the position ; Dimension update position The optimal individual in each dimension The method to obtain is as follows:
[0061]
[0062] (5)
[0063] in, is the fitness function; For particles No. adjacent particles; For particles No. The neighboring particles Dimensional individual; For particles No. dimensional particle position; is the optimal position of the dimension; Represents particles All neighboring particles of Indicates finding the position of the corresponding particle.
[0064] Update position based on dimension Update the optimal position of the dimension. The specific process is as follows: Get the updated position of the dimension respectively and optimal position of dimensions Corresponding fitness; if the dimension updates the position The corresponding fitness is greater than the optimal position of the dimension The corresponding fitness is updated with the dimension As the updated optimal position of the dimension; otherwise, the update fails. If the number of times the particle fails to update the optimal position of the dimension is greater than the preset iteration threshold T, several particles are randomly generated, and the dominance of the randomly generated particles is compared with the original particles. A particle is randomly selected from the dominated particles to replace the original particle.
[0065] 5-4. Figure 5 As shown, update the particle speed and position
[0066] 5-4-1. Randomly select particle a from particle a dimensions, according to the global optimal position Get the global update speed of particle a in the hth dimension , whose expression is:
[0067] (6)
[0068] in, is the inertia weight; is the velocity of particle a before the h-th dimension update; is the value of the global optimal position in the hth dimension; is the position of particle a in the hth dimension; .
[0069] 5-4-2. Randomly select from the dimensions of particle a whose velocity has not been updated Dimensions ( ), according to the optimal position of the dimension Get the dimension update speed of particle a in the e-th dimension , whose expression is:
[0070] (7)
[0071] in, is the value of the optimal position in the dimension e; .
[0072] 5-4-3. Randomly select from the dimensions of particle a whose velocity has not been updated Dimensions ( ), based on the historical optimal position Get the historical update speed of particle a in the gth dimension , whose expression is:
[0073] (8)
[0074] in, is the value of the historical optimal position in the gth dimension; .
[0075] 5-4-4. Based on global update speed , dimension update speed and historical update speed Get the updated velocity of particle a ; Get the updated position of the particle based on the updated velocity of the particle .
[0076] 5-5. Repeat steps 5-2 to 5-4 until the termination condition is reached, and the historical optimal position The corresponding decision variables are used as the final local path.
[0077] Step 6: The final local path of the local path optimization module is transmitted to the controller. The controller switches the working mode of the palletizing robot to the load mode and starts the palletizing robot to perform palletizing work. The ultrasonic obstacle avoidance device detects the position of obstacles and the palletizing robot arm in real time and makes corresponding adjustments.
[0078] Step 7: The task is completed, the palletizing task is completed, and the controller switches the working mode of the palletizing robot to the no-load mode.
Claims
1. A path planning method for an embodied intelligent loading and stacking robot based on pose decomposition, characterized by: The method includes: Obtain the transport vehicle information and the loaded cargo information, and set the placement of the loaded cargo in the carriage of the transport vehicle based on the transport vehicle information and the loaded cargo information; obtain the initial point and target point of each loaded cargo according to the placement position of the loaded cargo; obtain the global path of the end of the palletizing robot arm on the palletizing robot from the initial point and the target point through the path planning algorithm; obtain the initial local path of each joint on the palletizing robot arm according to the global path, and use the particle swarm algorithm to optimize the time required between adjacent path nodes in the initial local path corresponding to each joint to obtain the final local path; in the particle swarm algorithm, update the particle speed and position by setting the global optimal position, the historical optimal position and the dimensional optimal position; the palletizing robot performs palletizing according to the final local path.
2. The path planning method for an embodied intelligent loading and stacking robot based on posture decomposition according to claim 1, characterized in that: The method for obtaining the optimal position of the dimension is as follows: Get the adjacent particles of each particle; select the optimal individual of each dimension from all the adjacent particles of the particle to form the dimension update position; the optimal individual of each dimension in the dimension update position The method to obtain is as follows: in, is the fitness function; For particles No. adjacent particles; For particles No. The neighboring particles Dimensional individual; For particles No. dimensional particle position; is the optimal position of the dimension; Represents particles All neighboring particles of Indicates finding the position of the corresponding particle; D is the particle dimension; Update the dimension optimal position based on the dimension update position.
3. The path planning method for an embodied intelligent loading and stacking robot based on posture decomposition according to claim 2 is characterized by: The method for updating the dimension optimal position based on the dimension update position is as follows: respectively obtain the fitness corresponding to the dimension update position and the dimension optimal position; if the fitness corresponding to the dimension update position is greater than the fitness corresponding to the dimension optimal position, then use the dimension update position as the updated dimension optimal position; otherwise, the update fails; if the number of update failures is greater than a preset iteration number threshold, then randomly generate several particles, compare the randomly generated particles with the original particles for dominance, and randomly select one particle from the dominated particles to replace the original particle.
4. The path planning method for an embodied intelligent loading and stacking robot based on posture decomposition according to claim 2, characterized in that: The method for obtaining the adjacent particles is as follows: The particle position , if there are particles b Meet the conditions , then the particle b For particles a adjacent particles; among them, is the maximum distance between particles; is the distance threshold.
5. The path planning method for an embodied intelligent loading and stacking robot based on pose decomposition according to claim 1, characterized in that: The particle speed and position are updated as follows: The global update speed, dimensional update speed and historical update speed of the particle in different dimensions are obtained according to the global optimal position, dimensional optimal position and historical optimal position respectively; the dimensions corresponding to different speed updates are mutually exclusive; the updated speed is composed based on the global update speed, dimensional update speed and historical update speed; the updated position of the particle is obtained based on the updated speed of the particle.
6. The path planning method for an embodied intelligent loading and stacking robot based on pose decomposition according to claim 1, characterized in that: The method to obtain the final partial path is as follows: Initialize the particle swarm; set the fitness function and obtain the fitness of each particle based on the fitness function; select the global optimal position and the historical optimal position based on the fitness of the particle, and update the dimension optimal position; update the particle speed and position based on the global optimal position, the historical optimal position and the dimension optimal position; repeat the above process until the termination condition is reached, and take the particle corresponding to the historical optimal position as the final local path.
7. The path planning method for an embodied intelligent loading and stacking robot based on pose decomposition according to claim 6, characterized in that: The fitness function includes an optimization target and a penalty term; the optimization target is the time required for the path corresponding to the particle; and the penalty term is obtained based on the constraint equation of the joint.
8. The path planning method for an embodied intelligent loading and stacking robot based on pose decomposition according to claim 1, characterized in that: The method to obtain the initial local path is as follows: Parameter modeling of the palletizing robot is performed, and the global path is discretized in Cartesian space to obtain N The inverse kinematics calculation is performed on the nodes in the Cartesian space to obtain the position sequence of each joint in the joint space; the position sequence of each joint is optimized using the B-spline curve to obtain the initial local path.
9. The path planning method for an embodied intelligent loading and stacking robot based on pose decomposition according to claim 1, characterized in that: The transport vehicle information includes the type of carriage, body size and placement angle of the transport vehicle; the loaded cargo information includes the size of the loaded cargo.
10. A path planning system for an embodied intelligent loading and stacking robot based on posture decomposition, characterized by: Used to execute the embodied intelligent loading and palletizing robot path planning method based on posture decomposition as described in claim 1; the embodied intelligent loading and palletizing robot path planning system includes a palletizing robot and an environmental perception module, a controller, a wireless communication module, and a path generation module installed on the palletizing robot; the palletizing robot is provided with a palletizing robot arm including multiple joints; the environmental perception module includes a vision module and an ultrasonic obstacle avoidance device; the vision module is used to identify the transport vehicle information and the loaded cargo information; the ultrasonic obstacle avoidance device is used to detect the position of obstacles and the palletizing robot arm; the controller is used to control the palletizing robot to perform palletizing; the path generation module includes a global path planning module, a local path planning module and a local path optimization module; the global path planning module is used to generate the path of the end of the palletizing robot arm; the local path planning module is used to generate the path of each joint on the palletizing robot arm; the local path optimization module is used to optimize the path of each joint.