A multi-robot trajectory planning method based on multi-strategy chaotic ACO

Abstract

The application discloses a multi-robot arm trajectory planning method based on a multi-strategy chaotic ACO, and belongs to the technical field of robot trajectory planning. The method first establishes a six-dimensional pose transformation model containing a triangular clamping object and a DH kinematics model, fuses a robot connecting rod, a clamped workpiece and an environmental obstacle to construct a dynamic obstacle set; a multi-strategy artificial potential field is used to distinguish different obstacle types and to differentially calculate an obstacle avoidance distance, a sine chaotic factor is introduced in quasi-Newton inverse kinematics solving to inhibit joint rollover; a fitness function is constructed, an ACO-MOAPF-RCh hybrid algorithm is formed in combination with an ant colony algorithm; multi-robot cooperative optimization is completed through serial arm planning, real-time updating of dynamic obstacles and parallel output of trajectories. The method does not need artificial repeated demonstration debugging, the optimization performance of the algorithm is improved by 33.52% compared with a basic ant colony, the joint rollover frequency is greatly reduced, multi-robot workpiece collision-free high-precision carrying and posture placement can be realized, and the method is suitable for flexible manufacturing, logistics sorting and the like scenes.

Description

Technical Field

[0001] This invention relates to the fields of industrial robot control, intelligent algorithms, and robotic arm trajectory planning, specifically to a multi-robotic arm trajectory planning method based on multi-strategy chaotic ACO. Background Technology

[0002] With the upgrading of industrial automation, multi-robotic arm collaborative systems are widely used in flexible assembly, logistics sorting, and workpiece transfer. When multiple robotic arms are running, the links of each robotic arm and the workpiece held at the end effector form dynamically coupled obstacles, and trajectory planning is a multi-objective, dynamically constrained optimization problem.

[0003] Existing trajectory planning techniques have the following drawbacks:

[0004] 1. The modeling is simplified, treating only the end effector of the robotic arm as a single point without considering the shape and posture of the workpiece being gripped, which can easily lead to workpiece collisions during movement;

[0005] 2. Mature solutions are only compatible with single and dual robotic arms. There is a lack of optimization strategies for the collaboration of four or more robotic arms. In industrial settings, manual teaching and repeated debugging of each robotic arm is required, which is extremely time-consuming and labor-intensive.

[0006] 3. Numerical solutions for the inverse kinematics of a robotic arm are prone to singular solutions, causing significant joint rotation and poor motion stability;

[0007] 4. Using a uniform distance calculation method to handle all obstacles makes it impossible to distinguish between its own linkages, clamped workpieces, and external static obstacles, resulting in insufficient obstacle avoidance reliability;

[0008] 5. Traditional ant colony algorithms have weak local search capabilities and are prone to getting trapped in local optima in complex dynamic environments with multiple arms, resulting in redundant trajectories and slow convergence speed. Summary of the Invention

[0009] The purpose of this invention is to overcome the shortcomings of existing multi-arm trajectory planning methods, such as high manual debugging costs, weak dynamic obstacle avoidance capabilities, lack of end-effector posture planning, joint flipping caused by inverse kinematics singularities, and poor multi-arm cooperative adaptability. This invention provides a multi-arm trajectory planning method based on multi-strategy chaotic ACO, which automates the entire process of multi-arm trajectory planning, improves obstacle avoidance accuracy and motion stability, and reduces operating costs.

[0010] To achieve the above objectives, this invention proposes a multi-robot trajectory planning method based on multi-strategy chaotic ACO. Relying on an improved ant colony algorithm, multi-strategy obstacle avoidance technology, and chaotic optimization technology, it realizes fully automated trajectory planning, dynamic obstacle avoidance, and stable collaborative operation in scenarios where 4-6 six-degree-of-freedom robot arms are gripping a workpiece. Specifically, it includes:

[0011] S1 parameter initialization: Determine the world coordinates of the base of each six-DOF robotic arm, the DH parameters of the robotic arm, the vertex coordinate matrix of the object being held, and the set of static obstacles in the environment; determine the initial six-dimensional pose of the end effector and the target six-dimensional pose of each robotic arm; determine the ant colony algorithm parameters;

[0012] S2 modeling of the clamped object and its pose transformation: Define a right-angled triangle clamped object matrix, construct a six-dimensional step pose vector for each step at the end; construct a three-axis rotation matrix (xyz), and solve for the real-time world coordinates of all feature points of the clamped workpiece through a composite transformation of translation to the origin, pose rotation, and translation to the current position at the end.

[0013] S3 Multi-Manipulator Kinematic Modeling: Based on DH parameters, a DH transformation matrix is ​​constructed to obtain the forward kinematic model of the manipulator, and the actual positions of each joint and the actual positions of the vertices of the gripped object are calculated; the manipulator joint points and link obstacles, workpiece gripping obstacles, and static environmental obstacles are fused to generate a global dynamic obstacle set; quasi-Newton iteration is used to realize the numerical solution of inverse kinematics, and a random sinusoidal chaotic factor is introduced to correct the initial value of the inverse solution iteration;

[0014] The S4 single-arm robotic arm uses an ACO-MOAPF-RCh iterative optimization process. A single ant colony iteration includes: constructing a 3×3×3 three-dimensional local feasible region based on the current end-effector pose to generate a pre-selected end-effector pose; employing a multi-strategy artificial potential field method to apply differentiated distance calculation rules for different types of obstacles; traversing the pose changes to solve for the optimal pose increment and update the six-dimensional end-effector pose; using inverse kinematics with chaos factor correction to solve for the six joint angles corresponding to the pre-selected pose; calculating the single-ant path fitness function; updating the spatial pheromone using an ant cycle model; and outputting the sequence of key points and corresponding joint angles of the complete trajectory of the robotic arm after iterating to the maximum number of iterations.

[0015] S5 Multi-Robot Serial Planning: Step S4 is executed sequentially according to the robot arm number. After each robot arm trajectory planning is completed, the global dynamic obstacle set is refreshed based on the coordinates of each step joint and the workpiece being held by the robot arm, for use in subsequent robot arm planning.

[0016] S6 Collaborative Trajectory Output: After all robotic arms are planned, the fitness of each individual robotic arm is accumulated to obtain the overall system evaluation function; the trajectory points and joint angle sequences of all robotic arms are simultaneously sent out to control multiple robotic arms to collaboratively and in parallel to clamp the workpiece and move it to the target pose without collision.

[0017] In the aforementioned step S2, the clamped object Defined as a right triangle, the coordinate matrix of the vertices of the object being held is:

[0018] ,in These represent the coordinates of the three vertices of the right triangle, with the origin serving as the clamping reference point.

[0019] The end-effector six-dimensional stepping pose vector is: , This indicates the position of the machine end point relative to the robot arm base in the st-th step. This represents the rotational attitude angle of the end point of the robot arm relative to the robot arm base in the st-th step;

[0020] The xyz three-axis rotation matrix satisfies:

[0021] ;

[0022] Where S is the abbreviation for sin function and C is the abbreviation for cos function.

[0023] In step S3 above, the DH transformation matrix is:

[0024] ;

[0025] In the formula, ν is the radial angle between two adjacent joints, θ is the output angle of the servo motors of two adjacent joints, joint is the joint number, a is the link length, and d is the link offset length.

[0026] In step S3 above, the positive kinematic model of the robotic arm is a mathematical model for obtaining the spatial position and orientation of any joint, given the output angles of each joint, as shown in the following equation:

[0027] ;

[0028] in, It is the positive kinematic model of the robotic arm. This refers to the actual position and orientation of the robotic arm's end effector relative to world coordinates. For augmented matrices, Let be the coordinates of the base of the arth robotic arm, since It only has three coordinates (x, y, z), does not contain attitude information, and uses a 1×3 zero vector. Add it to 6 dimensions. These are the output angles of the six joints of a six-degree-of-freedom robotic arm. It is the DH transformation matrix;

[0029] The inverse kinematic model of a robotic arm is a mathematical model for solving the output angles of each joint given the spatial position and orientation of the end effector. Since the inverse model has no analytical solution, it is replaced by a numerical solution obtained through Quasi-Newton iteration. The inverse solution formula is... As shown in the following formula, the previous seed is added when solving the inverse solution. To avoid abrupt changes in the inverse solution of two adjacent steps:

[0030] ;

[0031] Because the current inverse solution of the robotic arm is different from the previous inverse solution... To prevent the robotic arm from flipping due to singular values ​​in the inverse solution, a 70% chaos factor is introduced into the inverse solution at each iteration. The improved inverse solution calculation process is shown in the following formula:

[0032] ;

[0033] The random and sinic chaotic strategies introduced by ACO-MOAPF-RCh, and their abbreviations, are shown in the following equations, where R represents random and Ch represents chaos:

[0034] .

[0035] In step S4 above, the multi-strategy obstacle distance between the robotic arm and the obstacle is as follows:

[0036] ;

[0037] in, It is the change in the pre-selected position of the robotic arm's end effector within the feasible region. This is the squared distance from the pre-selected location to the target location. To avoid a result of 0 that could lead to infinity in subsequent division operations, a positive minimum value is added here. , It is the square of the distance between the pre-selected location and each obstacle. It is the transition probability of the pre-selected position. This indicates an obstacle that is part of another robotic arm. yes The formed rod-shaped obstacle; This refers to an obstacle that is part of the object being held by another robotic arm. for The resulting triangular obstacle; This indicates an obstacle that is part of the first to fourth joints of the arth robotic arm; This refers to an obstacle belonging to the 5th joint of the arth robotic arm up to the end effector or part of the object it grips; when Too close to its own end effector, or too close to an obstacle ( When ), the distance is set to ; This indicates obstacles in the environment. Let t be the pheromone concentration at spatial location xyz during iteration t. These represent the target heuristic factor, obstacle heuristic factor, and pheromone heuristic factor, respectively.

[0038] In the aforementioned method, when the end attitude change amount The algorithm obtains the optimal end-effector attitude change when the sum of ten times the square of the distance between the end-effector attitude and the target attitude and the square of the changes in each joint is minimized. The calculation method is shown in the following formula:

[0039] ;

[0040] The new end-transfer position is thus obtained, as shown in the following formula:

[0041] ;

[0042] Global fitness function for a single robotic arm Used to update pheromone concentration; the smaller the value, the better the effect. The calculation method is shown in the following formula, where ST is a constant corresponding to a maximum value of 15. The final distance is determined by the L2 norm of two adjacent pre-selected positions. Summation yields the result; The difference between the final position and the target position includes the positional difference. and attitude gap , The fitness function is the square of the L2 norm of the angle changes of the six joints. Because the inverse kinematics of the robotic arm exhibits spatial symmetry, the fitness function incorporates the average height of each joint to minimize the proximity of the robotic arm joints to the base. As the denominator:

[0043] ;

[0044] The pheromone is updated using the ant-week model, with each ant carrying it once. =10 total pheromones. To minimize fitness, use the total pheromone amount. Divide by the global fitness of a single robotic arm The business The increase in pheromone caused by the nth ant in the tth iteration The pheromone update process is shown in the following formula:

[0045] ;

[0046] in, =0.1 is the pheromone evaporation rate, (x,y,z) is the position coordinate in the world coordinate system, n is the ant's number, t is the iteration number, and Time is the maximum number of iterations.

[0047] In step S6 above, the overall system evaluation function is:

[0048] ;

[0049] AR represents the total number of robotic arms, which can be 4, 5, or 6. Let αr be the single-arm fitness after the αr-th robotic arm has been iterated to the maximum number of times.

[0050] This method is applicable to scenarios involving 4 to 6 six-degree-of-freedom robotic arms in collaborative flexible manufacturing, automated logistics sorting, multi-robot workpiece handling, and collaborative assembly. Each robotic arm independently grips the workpiece, moves from different starting points to its own independent target pose, and completes attitude calibration.

[0051] This invention is applicable to collaborative handling, sorting, and assembly operations of workpieces held by 4 to 6 six-degree-of-freedom robotic arms. Firstly, it achieves joint modeling of the robotic arm and the held object, simultaneously constraining the position and attitude of the robotic arm's end effector, perfectly adapting to the actual working conditions of workpiece holding and simultaneously meeting the dual planning requirements of position and attitude. Secondly, it categorizes obstacles in the working environment, robotic arm links, and the held object, employing differentiated distance calculation strategies to accurately identify dynamically coupled obstacles, effectively avoiding collision risks between the robotic arm and the held object, and significantly improving dynamic obstacle avoidance performance.

[0052] This invention integrates the Multi-Strategy Artificial Potential Field (MOAPF) method and the Ant Colony Algorithm (ACO), and superimposes a random chaotic strategy to construct an improved ACO-MOAPF-RCh algorithm, balancing global optimization and local obstacle avoidance capabilities. Testing shows that this algorithm improves performance by 32.77% compared to the ACO-MOAPF algorithm without a chaotic factor, and by 33.52% compared to the basic ant colony algorithm, demonstrating significant optimization of global optimization. In the inverse kinematics solution of the robotic arm, this invention introduces a chaotic factor accounting for 70%, combined with the preceding step inverse solution as a seed value, effectively suppressing singular values. In the four-arm robotic system, the joint flipping frequency is reduced from 47.78% to 14.00%, greatly improving the motion stability of the robotic arm.

[0053] Meanwhile, this invention employs a multi-arm collaborative logic of serial planning, dynamic obstacle updating, and parallel execution, requiring no human intervention throughout the entire process. It completely eliminates the traditional manual teaching and repeated debugging mode, achieving fully automated trajectory planning, significantly reducing labor and time costs, and greatly improving planning efficiency. This method can simultaneously adapt to the collaborative operation of 4 to 6 six-degree-of-freedom robotic arms, with a wide range of applications, and can be extended to various industrial scenarios such as automated production lines, logistics sorting, and flexible assembly. Attached Figure Description

[0054] Figure 1 This is a simulation model of a six-degree-of-freedom robotic arm.

[0055] Figure 2 This is a schematic diagram of local search for path planning of a single robotic arm. The pentagram represents the current position and orientation of the robotic arm's end effector. The space enclosed by the cube is the feasible region for local search of the robotic arm, which is actually a 3x3x3 area centered on the current position. The goal includes the position and orientation target. The obstacles mainly consist of multiple robotic arms (including themselves) and the objects they hold.

[0056] Figure 3 Flowchart of trajectory planning algorithm for multi-robotic arm system;

[0057] Figures 4 to 7 They are respectively Figure 3 The flowcharts for parts (a), (b), (c), and (d) are as follows;

[0058] Figure 8 Simulation diagram of trajectory planning for a four-arm robotic system;

[0059] Figure 9 Simulation diagram of trajectory planning for a five-arm robotic system;

[0060] Figure 10 Simulation diagram of trajectory planning for a six-arm robotic system;

[0061] in, Figures 8 to 10 Each step contains 15 motion steps (equivalent to 15 teaching and reproduction key points), where the black triangle represents the object being held and the red circle represents the inherent obstacles in the environment. Detailed Implementation

[0062] The technical solution of the present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0063] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0064] Example

[0065] This embodiment defines multi-arm trajectory planning as a multi-task dynamic trajectory planning problem with six-degree-of-freedom pose constraints. Based on the kinematic model of a single-arm robot, a fitness function for the multi-arm trajectory is constructed. A random chaotic strategy (RCh) is fused using the ACO-MOAPF (ant colony algorithm + artificial potential field with multi-obstacle recognition strategy) algorithm to achieve conflict-free transport and precise placement of multiple (4-6) six-degree-of-freedom robots along the planned trajectory while the end effector is holding the workpiece. Simulation results show that the ACO-MOAPF-RCh method improves performance by 32.77% and 33.52% compared to ACO-MOAPF without chaos factor and basic ACO, respectively. In a four-arm system, the joint flipping frequency is reduced from 47.78% to 14.00% compared to ACO-MOAPF without chaos factor. In five-arm and six-arm systems, ACO-MOAPF-RCh demonstrates its effectiveness and potential in multi-arm trajectory planning, specifically in cases where the held object... Set as a right triangle, with the right-angle vertex (0,0,0) being the clamped point, and Pr is defined as shown in equation (1).

[0066] (1)

[0067] in These represent the coordinates of the three vertices of the right triangle.

[0068] Using a vector P with six degrees of freedom st The state of the end point of the robotic arm relative to the base of the robotic arm is described as shown in Equation (2). The first 1 to 3 terms (1:3) are the spatial position of the end point, and the 4 to 6 terms (4:6) are the spatial orientation of the end point, where st represents the number of steps.

[0069] (2)

[0070] in This indicates the position of the machine end point relative to the robot arm base in the st-th step. This indicates the orientation of the end point of the robotic arm relative to the robotic arm base at step t.

[0071] The initial position and orientation of the end point of the robotic arm relative to the base of the robotic arm are shown in equation (3).

[0072] (3)

[0073] in This indicates the initial position of the mechanical end point relative to the robot arm base in step 1. This indicates the initial posture of the end point of the robotic arm relative to the robotic arm base in step 1.

[0074] The final position and posture of the end point of the robotic arm are shown in equation (4).

[0075] (4)

[0076] in This indicates the final position of the mechanical end point relative to the robot arm base. This indicates the final posture of the end effector of the robotic arm relative to its base.

[0077] The order of the rotation axes of the clamped object is x, y, z, and the rotation angles are γ, λ, φ, respectively, in degrees. C is the abbreviation for cos and S is the abbreviation for sine function, as shown in equation (5).

[0078] (5)

[0079] The final rotation transformation matrix around the xyz axes is: .

[0080] To avoid the influence of spatial displacement on the rotation radius, the clamping point must first be translated back to the origin, then rotated, and finally translated to the designated position. As shown in equation (6), where i represents the ordinal number of the feature point of the clamped object, and for each feature point on the clamped object, the actual coordinates after translation and rotation are: As shown in equation (6), where the superscript T represents matrix transpose.

[0081] (6)

[0082] in Represents the various feature points of the object being gripped at the end of the robotic arm in the st-th step. By angle Perform rotation around the xyz axis.

[0083] 3. Kinematic Model of Robotic Arm

[0084] For a system consisting of multiple robotic arms, although each robotic arm has the same structure, their base spatial positions are different. This embodiment assumes the coordinates of the robotic arm bases are... As shown in equation (7), where ɑr is the serial number of the robotic arm, It represents its specific spatial location.

[0085] (7)

[0086] The actual position and orientation of the robotic arm's end effector relative to world coordinates As shown in equation (8), where st represents the step sequence number, ST is a constant corresponding to the maximum value of 15, and Joint is the maximum number of joints (i.e., the corresponding end point). Corresponding to the positions respectively Each corresponds to a different posture.

[0087] (8)

[0088] Specifically, the actual starting position and posture of the robotic arm's end effector. and actual final position and attitude As shown in equation (9), where the superscript This represents the desired position, where 1:3 represents the position coordinates and 4:6 represents the attitude angles. This indicates the location of the clamping point of the object being clamped.

[0089] (9)

[0090] As the robotic arm moves with the object it grips, each joint and the vertex position of the gripped object become new obstacles. The objectives to be optimized include the distance traveled, the difference between the final position and orientation and the target position and orientation, the variance of the changes in the joints during movement, and the closest distance to obstacles during the movement.

[0091] The simulation model of the six-degree-of-freedom robotic arm is shown in the figure. Figure 1 As shown in Table 1, this is a table of DH parameters. To accommodate different grid sizes, the length unit is dm and the angle unit is degrees.

[0092] Table 1. Parameter Table for a Six-DOF Robotic Arm (DH)

[0093]

[0094] DH transformation matrix As shown in equation (10), where S is the abbreviation for sin function, C is the abbreviation for cos function, ν is the radial angle between two adjacent joints, θ is the output angle of the servo motors of two adjacent joints, and joint is the joint number.

[0095] (10)

[0096] The positive kinematic model of a robotic arm is a mathematical model for obtaining the spatial position and orientation of any joint under the premise of knowing the output angle of each joint, as shown in Equation (11).

[0097] (11)

[0098] in For an augmented matrix, since It only has three coordinates (x, y, z), does not contain attitude information, and uses a 1×3 zero vector. Add it to 6 dimensions. It is the positive kinematic model of the robotic arm. These are the output angles of the six joints of a six-degree-of-freedom robotic arm.

[0099] The inverse kinematics model of a robotic arm is a mathematical model for solving the output angles of each joint given the spatial position and orientation of the end effector. Since the inverse model has no analytical solution, it is replaced by a numerical solution obtained through Quasi-Newton iterations. The inverse solution formula is... As shown in equation (12). The previous seed is added when solving the inverse solution. This is to avoid abrupt changes in the inverse solutions of two adjacent steps.

[0100] (12)

[0101] Because the current inverse solution of the robotic arm is different from the previous inverse solution... To prevent the robotic arm from flipping due to singular values ​​in the inverse solution, we introduce a 70% chaos factor into the inverse solution at each iteration. The improved inverse solution calculation process is shown in equation (13).

[0102] (13)

[0103] The random and sin chaotic strategies introduced by ACO-MOAPF-RCh, and their abbreviations, are shown in Equation (14), where R represents random and Ch represents chaos.

[0104] (14)

[0105] 4. Robotic arm end-point trajectory planning

[0106] Multi-arm trajectory planning: For AR robotic arms, plan AR trajectories such that the end effector (numbered Joint) moves along... (Including position and orientation) movement, causing the clamped object to move from different starting points. Reaching different destinations And adjust its posture accordingly. During the movement, the various joints of the robotic arm... and the clamped object This will create new obstacles. It's worth noting that, to improve code efficiency, we omitted the square root calculation step when calculating the Euclidean distance, and directly used the square of the distance as the evaluation value for distance.

[0107] like Figure 2As shown, the path planning of the robotic arm exhibits obvious local and global characteristics. By repeatedly performing local searches within the feasible region (i.e., the space enclosed by the cube in the figure), a series of locally optimal solutions are obtained and combined into a path. Then, global optimization is performed to obtain an approximate globally optimal solution. This process is highly consistent with the core idea of ​​the ant colony algorithm, which involves "local random migration of individual ants and global optimization using pheromones from the ant colony." Therefore, this embodiment selects the ant colony algorithm as the main algorithm for path planning.

[0108] To prevent the robotic arm from colliding with obstacles while being held by the object and to ensure that the robotic arm moves towards the goal, this embodiment introduces the Human Multi-Strategy Obstacle Avoidance Potential Field (MOAPF) method for collision detection and goal heuristics. This strategy employs different methods for calculating obstacle distances for different types of obstacles to achieve effective obstacle avoidance. In subsequent experiments, given the significant impact of the initial state on the inverse solution, this embodiment introduces a chaos factor during the state initialization phase to optimize the initial state.

[0109] like Figure 2 As shown, where, It is the change in the pre-selected position of the robotic arm's end effector within the feasible region. It is the pre-selected position and orientation (complete coordinate point) in the feasible region. This is the squared distance from the pre-selected location to the target location. To avoid a result of 0 that could lead to infinity in subsequent division operations, a positive minimum value is added here. , It is the square of the distance between the pre-selected location and each obstacle. This refers to the transition probability of a pre-selected location. A point is randomly selected based on the transition probability. The calculation process for the end-point transfer position is shown in Equation (15). The calculation process and the multi-strategy obstacle distance between the robotic arm and the obstacle are shown in Equation (15).

[0110] (15);

[0111] in, It is the change in the pre-selected position of the robotic arm's end effector within the feasible region. This is the squared distance from the pre-selected location to the target location. To avoid a result of 0 that could lead to infinity in subsequent division operations, a positive minimum value is added here. , It is the square of the distance between the pre-selected location and each obstacle. It is the transition probability of the pre-selected position. This indicates an obstacle that is part of another robotic arm. yes The formed rod-shaped obstacle; This refers to an obstacle that is part of the object being held by another robotic arm. for The resulting triangular obstacle; This indicates an obstacle that is part of the first to fourth joints of the arth robotic arm; This refers to an obstacle belonging to the 5th joint of the arth robotic arm up to the end effector or part of the object it grips; when Too close to its own end effector, or too close to an obstacle ( When ), the distance is set to ; This indicates obstacles in the environment. Let t be the pheromone concentration at spatial location xyz during iteration t. These represent the target heuristic factor, obstacle heuristic factor, and pheromone heuristic factor, respectively.

[0112] For the end attitude change, when The algorithm obtains the optimal end-effector attitude change when the sum of ten times the square of the distance between the end-effector attitude and the target attitude and the square of the changes in each joint is minimized. The calculation method is shown in equation (16):

[0113] (16)

[0114] in From the starting position and attitude attitude at target position The average change, where st is the total number of steps. End attitude change Evaluation function;

[0115] The new end-transfer position is thus obtained, as shown in equation (17):

[0116] (17)

[0117] Global fitness function for a single robotic arm Used to update pheromone concentration, the smaller the value, the more ideal the effect. The calculation method is shown in equation (18), where, The final distance is determined by the L2 norm of two adjacent pre-selected positions. Summation yields the result; The actual location of the endpoint and actual posture Compared to the target location With target attitude The error is due to the fact that taking the square root would consume a lot of computation, while not taking the square root would not affect subsequent calculations. Therefore, to improve efficiency, the square root is not taken. This also includes the positional difference in the difference between the final position and the target position. and attitude gap , This is the square of the L2 norm of the angle changes at the six joints. Because the inverse kinematics of the robotic arm exhibits spatial symmetry, the fitness function incorporates the average height of each joint to minimize the proximity of the robotic arm joints to the base. , as the denominator.

[0118] (18)

[0119] This embodiment uses the ant-period model to update pheromones, with a single ant carrying them at a time. =10 total pheromones. To minimize fitness, use the total pheromone amount. Divide by the global fitness of a single robotic arm The business The increase in pheromone caused by the nth ant in the tth iteration The pheromone update process is shown in equation (19):

[0120] (19)

[0121] in, =0.1 is the pheromone evaporation rate, (x,y,z) are the position coordinates in the world coordinate system, n is the ant number, t is the iteration number, and Time is the maximum iteration number. In order to evaluate the overall quality of the multi-manipulator system path, we sum the single-manipulator fitness of multiple manipulators to obtain the multi-manipulator system evaluation function Fit, as shown in Equation (20).

[0122] (20)

[0123] During trajectory planning, the algorithm operates serially, planning the motion trajectories of each robotic arm in turn. Once the trajectory of one robotic arm is determined, the system updates the obstacles in real time based on the position information of each joint and end effector at each step, ensuring the dynamic changes of the obstacles are addressed. Subsequent robotic arms will then plan their own paths based on this dynamic obstacle information. Ultimately, the different paths planned by the algorithm for different robotic arms are mutually constrained and can be executed simultaneously in parallel.

[0124] like Figure 3 The diagram shows the flowchart of the trajectory planning algorithm for a multi-robotic arm system. This algorithm outputs multiple robotic arm trajectories. , Corresponding inverse kinematic solution And fitness (Fit). The algorithm consists of four functional modules.

[0125] Figure 4 (Right now Figure 3 Part (a) in the diagram is the main system flowchart. It is used for planning from... arrive The trajectory. Where ɑr is the robotic arm serial number, and AR is the total number of robotic arms. This represents the arth robotic arm. The object held by the arth robotic arm. Let be the starting position and orientation of the end effector of the r-th robotic arm. Let the target positioning and attitude of the arth robotic arm's end effector be determined. This is the position of the base of the arth robotic arm.

[0126] Figure 5 (Right now Figure 3 Part (b) is the sub-module flowchart. Used for planning. The trajectory. Among them, For local fitness, This represents the world coordinate system position and orientation of each joint of the arth robotic arm at the stth step.

[0127] Figure 6 (Right now Figure 3 Part (c) is the submodule flowchart. It is used to plan the trajectory of a single robotic arm using ACO-MOAPF-RCh. The pheromone increase is represented by x, y, z, which represent the position coordinates in the world coordinate system, n is the number of ants, t is the number of iterations, and Time is the maximum number of iterations.

[0128] Figure 7 (Right now Figure 3 Part (d) in the diagram is the flowchart for the submodule. This submodule is used to search within the feasible region. , It is a pre-selected position and orientation (complete coordinate point) within the feasible region; It is about The inverse kinematic solution; It is the transition probability of the pre-selected position; according to formulas (14) and (15), using calculate ; If the robot gets stuck in a local singularity, the joints of the robotic arm may flip. This can be achieved through local search calculations. To reduce the probability of this phenomenon occurring, we introduced a chaos factor. Finally, we output the output for each robotic arm. and ;in to Key points on the trajectory; This is the inverse kinematics solution for each key point on the trajectory.

[0129] The above specific embodiments are merely several optional embodiments of the present invention. Based on the technical solutions of the present invention and the relevant teachings of the above embodiments, those skilled in the art can make various alternative improvements and combinations to the above specific embodiments.

[0130] The above description of the disclosed embodiments enables those skilled in the art to make or use this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined in the embodiments of this application may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown in this application, but is to be accorded the widest scope consistent with the principles and novel features disclosed in the embodiments of this application.

Claims

1. A multi-robot trajectory planning method based on multi-strategy chaotic ACO, characterized in that, Includes the following steps: S1 parameter initialization: Determine the world coordinates of the base of each six-DOF robotic arm, the DH parameters of the robotic arm, the vertex coordinate matrix of the object being held, and the set of static obstacles in the environment; determine the initial six-dimensional pose of the end effector and the target six-dimensional pose of each robotic arm; determine the ant colony algorithm parameters; S2 Modeling of Clamped Object and Pose Transformation: Define the clamped object matrix and construct a six-dimensional step pose vector for each step at the end; construct a three-axis rotation matrix (xyz), and solve for the real-time world coordinates of all feature points of the clamped workpiece through a composite transformation of translation to the origin, posture rotation, and translation to the current position at the end. S3 Multi-Manipulator Kinematic Modeling: Based on DH parameters, a DH transformation matrix is ​​constructed to obtain the forward kinematic model of the manipulator, and the actual positions of each joint and the actual positions of the vertices of the gripped object are calculated; the manipulator joint points and link obstacles, workpiece gripping obstacles, and static environmental obstacles are fused to generate a global dynamic obstacle set; quasi-Newton iteration is used to realize the numerical solution of inverse kinematics, and a random sinusoidal chaotic factor is introduced to correct the initial value of the inverse solution iteration; S4 single robotic arm ACO-MOAPF-RCh iterative optimization, a single ant colony iteration includes: constructing a 3×3×3 three-dimensional local feasible region with the current end-effector pose, and generating a pre-selected end-effector pose; A multi-strategy artificial potential field method is adopted to apply differentiated distance calculation rules for different types of obstacles; the optimal posture increment is solved by traversing the posture changes and the end-effector six-dimensional pose is updated; the six joint angles corresponding to the pre-selected pose are solved by inverse kinematics with chaos factor correction; the fitness function of the single ant path is calculated; the spatial pheromone is updated by using the ant cycle model; after iterating to the maximum number of iterations, the key points of the complete trajectory of the robotic arm and the corresponding joint angle sequence are output. S5 Multi-Robot Serial Planning: Step S4 is executed sequentially according to the robot arm number. After each robot arm trajectory planning is completed, the global dynamic obstacle set is refreshed based on the coordinates of each step joint and the workpiece being held by the robot arm, for use in subsequent robot arm planning. S6 Collaborative Trajectory Output: After all robotic arms are planned, the fitness of each individual robotic arm is accumulated to obtain the overall system evaluation function; the trajectory points and joint angle sequences of all robotic arms are simultaneously sent out to control multiple robotic arms to collaboratively and in parallel to clamp the workpiece and move it to the target pose without collision.

2. The multi-robot trajectory planning method based on multi-strategy chaotic ACO according to claim 1, characterized in that, In step S2, the clamped object Defined as a right triangle, the coordinate matrix of the vertices of the object being held is: ,in These represent the coordinates of the three vertices of the right triangle, with the origin serving as the clamping reference point. The end-effector six-dimensional stepping pose vector is: , This indicates the position of the machine end point relative to the robot arm base in the st-th step. This represents the rotational attitude angle of the end point of the robot arm relative to the robot arm base in the st-th step; The xyz three-axis rotation matrix satisfies: ； Where S is the abbreviation for sin function and C is the abbreviation for cos function.

3. The multi-robot trajectory planning method based on multi-strategy chaotic ACO according to claim 1, characterized in that, DH transformation matrix: ； In the formula, ν is the radial angle between two adjacent joints, θ is the output angle of the servo motors of two adjacent joints, joint is the joint number, a is the link length, and d is the link offset length.

4. The multi-robot trajectory planning method based on multi-strategy chaotic ACO according to claim 1, characterized in that, The positive kinematic model of a robotic arm is a mathematical model for determining the spatial position and orientation of any joint, given the output angles of each joint, as shown in the following equation: ； in, It is the positive kinematic model of the robotic arm. This refers to the actual position and orientation of the robotic arm's end effector relative to world coordinates. For augmented matrix, Let be the coordinates of the base of the arth robotic arm, since It only has three coordinates (x, y, z), does not contain attitude information, and uses a 1×3 zero vector. Add it to 6 dimensions. These are the output angles of the six joints of a six-degree-of-freedom robotic arm. It is the DH transformation matrix, which is calculated from the positive kinematic model of the robotic arm to determine the actual position of each joint and the actual position of the vertex of the object being gripped. The inverse kinematic model of a robotic arm is a mathematical model for solving the output angles of each joint given the spatial position and orientation of the end effector. Since the inverse model has no analytical solution, it is replaced by a numerical solution obtained through Quasi-Newton iteration. The inverse solution formula is... As shown in the following formula, the previous seed is added when solving the inverse solution. To avoid abrupt changes in the inverse solution of two adjacent steps: ； Because the current inverse solution of the robotic arm is different from the previous inverse solution... To prevent the robotic arm from flipping due to singular values ​​in the inverse solution, a 70% chaos factor is introduced into the inverse solution at each iteration. The improved inverse solution calculation process is shown in the following formula: ； The stochastic and sinic chaotic strategies introduced by ACO-MOAPF-RCh, and their abbreviations, are shown in the following equations: ； Where R represents randomness and Ch represents chaos. The inverse solution represents the chaotic component in the reference value. It is a 1×6 random vector with values ​​from 0 to 1. It is a 1×6 unit vector.

5. The multi-robot trajectory planning method based on multi-strategy chaotic ACO according to claim 1, characterized in that, In step S4, the multi-strategy obstacle distance between the robotic arm and the obstacle is as follows: ； in, It is the change in the pre-selected position of the robotic arm's end effector within the feasible region. This is the squared distance from the pre-selected location to the target location. To avoid a result of 0 that could lead to infinity in subsequent division operations, a positive minimum value is added here. , It is the square of the distance between the pre-selected location and each obstacle. It is the transition probability of the pre-selected position. This indicates an obstacle that is part of another robotic arm. yes The formed rod-shaped obstacle; This refers to an obstacle that is part of the object being held by another robotic arm. for The resulting triangular obstacle; This indicates an obstacle that is part of the first to fourth joints of the arth robotic arm; This refers to an obstacle belonging to the 5th joint of the arth robotic arm up to the end effector or part of the object it grips; when Too close to its own end effector, or too close to an obstacle ( When ), the distance is set to ; This refers to obstacles in the environment. Let t be the pheromone concentration at spatial location xyz during iteration t. These represent the target heuristic factor, obstacle heuristic factor, and pheromone heuristic factor, respectively.

6. The multi-robot trajectory planning method based on multi-strategy chaotic ACO according to claim 5, characterized in that, When the end attitude change The algorithm obtains the optimal end-effector attitude change when the sum of ten times the square of the distance between the end-effector attitude and the target attitude and the square of the changes in each joint is minimized. The calculation method is shown in the following formula: ； in From the starting position and attitude attitude at target position The average change, st is the total number of steps, and ST is the constant corresponding to the maximum value of 15. End attitude change Evaluation function; The new end-transfer position is thus obtained, as shown in the following formula: ； Global fitness function for a single robotic arm Used to update pheromone concentration, a smaller value yields a better effect. The calculation method is shown in the following formula, where... The final distance is determined by the L2 norm of two adjacent pre-selected positions. Summation yields the result; The actual location of the endpoint and actual posture Compared to the target location With target attitude The error is due to the fact that taking the square root would consume a lot of computation, while not taking the square root would not affect subsequent calculations. Therefore, to improve efficiency, the square root is not taken. The fitness function is the square of the L2 norm of the angle changes of the six joints. Because the inverse kinematics of the robotic arm exhibits spatial symmetry, the fitness function incorporates the average height of each joint to minimize the proximity of the robotic arm joints to the base. As the denominator: ； The pheromone is updated using the ant-week model, with each ant carrying it at one time. =10 total pheromones. To minimize fitness, use the total pheromone amount. Divide by the global fitness of a single robotic arm The business The increase in pheromone caused by the nth ant in the tth iteration The pheromone update process is shown in the following formula: ； in, =0.1 is the pheromone evaporation rate, (x,y,z) is the position coordinate in the world coordinate system, n is the ant's number, t is the iteration number, and Time is the maximum number of iterations.

7. The multi-robot trajectory planning method based on multi-strategy chaotic ACO according to claim 1, characterized in that, The overall system evaluation function in step S6 is: ； AR represents the total number of robotic arms, which can be 4, 5, or 6. Let αr be the single-arm fitness after the αr-th robotic arm has been iterated to the maximum number of times.

8. The multi-robot trajectory planning method based on multi-strategy chaotic ACO according to claim 1, characterized in that, This method is applicable to scenarios involving 4 to 6 six-degree-of-freedom robotic arms in collaborative flexible manufacturing, automated logistics sorting, multi-robot workpiece handling, and collaborative assembly. Each robotic arm independently grips the workpiece, moves from different starting points to its own independent target pose, and completes attitude calibration.

Images