Obstacle avoidance method for industrial robot in complex environment

Abstract

Disclosed in the present invention is an obstacle avoidance method for an industrial robot in a complex environment, comprising the following steps: S1, performing model simplification on an industrial robot in a working space, and simplifying an obstacle into an obstacle point cloud; S2, placing a depth camera outside a motion space of the industrial robot in an "eye-to-hand" arrangement mode; and S3, before the industrial robot moves, using a G-APF-RRT* algorithm to plan a feasible path in the working space. In the present invention, the calculation mode of the distance between the industrial robot and the obstacle is simple, the required time is short, virtual joint interpolation is used and a spherical bounding box is used at a joint to completely envelope the industrial robot so as to simplify a model, and distance calculation can be completed only by calculating the distance between the center of sphere of the limited spherical bounding box and each point in the obstacle point cloud.

Description

An obstacle avoidance method for industrial robots in complex environments Technical Field

[0001] The present invention relates to the technical field of industrial robots, and in particular to an obstacle avoidance method for industrial robots in complex environments. Background Art

[0002] As the application areas of industrial robots continue to expand, people's requirements for the coordination, stability, and efficiency of industrial robots are increasing. Among them, the obstacle avoidance ability and work efficiency of industrial robots have received widespread attention.

[0003] At present, there is no mature, highly applicable technical solution that can be truly applied to industrial production to solve the problem of how industrial robots can avoid obstacles in complex environments while shortening their running time as much as possible to improve work efficiency. The main problems with how industrial robots can avoid obstacles while shortening their running time include: due to the large number of joints, strong flexibility, and diverse postures of industrial robots, they cannot be simply regarded as a point mass for obstacle avoidance. Therefore, when completing obstacle avoidance at the end of the industrial robot, it is also necessary to consider whether the industrial robot joints collide with obstacles; the motion planning process of industrial robots is numerous and complex, which makes the planning time long; the quality of trajectory planning has a great influence on the movement time required to complete the action.

[0004] Summary of the Invention

[0005] The purpose of the present invention is to provide an obstacle avoidance method for industrial robots in complex environments to solve the problems raised in the above background technology. For example, due to the large number of joints, strong flexibility, and various postures of industrial robots, they cannot be simply regarded as a point mass for obstacle avoidance. Therefore, while completing the obstacle avoidance at the end of the industrial robot, it is also necessary to consider that the joints of the industrial robot do not collide with obstacles; the motion planning process of the industrial robot is numerous and complex, which makes the planning time long; the quality of trajectory planning has a great influence on the motion time required to complete the action.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a method for industrial robot obstacle avoidance in a complex environment, comprising the following steps:

[0007] S1. Simplify the model of the industrial robot in the workspace and simplify the obstacles into obstacle point clouds;

[0008] S2, using the “eyes outside the hands” arrangement to place the depth camera outside the motion space of the industrial robot;

[0009] S3, before the industrial robot moves, a feasible path is planned in the workspace using the G-APF-RRT* algorithm;

[0010] S4. To avoid collisions between the joints of the industrial robot and obstacles when the end of the industrial robot moves along the path, the newly generated tree node q new Perform joint collision detection to obtain a complete collision-free path for the industrial robot to move from the starting point to the target point;

[0011] S5. Perform inverse kinematics calculation on the obtained collision-free complete path to obtain the joint positions of each joint of the industrial robot in the joint space, i.e., the interpolated positions, which are used for subsequent trajectory interpolation. The joint space trajectory of the industrial robot is planned using the 3-5-3 polynomial interpolation method to obtain the joint space position-time trajectory curve, angular velocity-time trajectory curve, and angular acceleration-time trajectory curve of the industrial robot;

[0012] S6. Establish the optimization objective function and trajectory constraints based on the joint space trajectory equation to prepare for the subsequent time-optimal trajectory planning;

[0013] S7, using the improved particle swarm optimization algorithm, the running time t of the three segments of the 3-5-3 polynomial interpolation trajectory is calculated. j1 , t j2 , t j3 Optimize the time for each joint of the industrial robot to run along the trajectory to the shortest possible extent, and satisfy the joint angular velocity v during operation. ji , angular acceleration a ji Does not exceed the constraint value v max and a max ;

[0014] S8, complete the trajectory optimization of all joints and perform joint synchronization processing;

[0015] S9, using the interpolation times t1, t2, and t3 after synchronization processing to perform 3-5-3 polynomial interpolation on each joint to obtain the time optimal trajectory of each joint;

[0016] S10. Send the joint space trajectory to the controller to control the industrial robot to move along the trajectory, avoid obstacles and reach the target point.

[0017] Preferably, in step S1, the industrial robot model simplification method is: determining the real joint points of the industrial robot, interpolating virtual joint points at a certain distance on the connecting rods between adjacent real joint points, and then using a spherical bounding box with the joint points as the center of the sphere to completely envelop the industrial robot body, ensuring that the industrial robot can be completely replaced when it is in any posture.

[0018] Preferably, in step S2, point cloud information of the industrial robot and obstacles in the workspace is obtained before path planning, and the point cloud of the industrial robot body is filtered out using a point cloud filter, leaving only the point cloud of the obstacle in the workspace.

[0019] Preferably, in step S3, the G-APF-RRT* algorithm is an improved rapidly expanding random tree algorithm, which is referred to as the RRT algorithm. The rapidly expanding random tree algorithm connects random sampling points based on a random sampling method in the workspace to form a feasible path, and its connection process is similar to the growth of a tree.

[0020] Preferably, in step S4, the process of joint collision detection is as follows: solve the position information of each joint when the end of the industrial robot is located at the newly generated tree node by inverse kinematics, calculate the Euclidean distance D between the center of each enclosing ball of the industrial robot and the obstacle point cloud in the base coordinate system ji , obtain the shortest distance and compare it with the safety distance. If the shortest distance is greater than the set safety distance, retain the newly generated tree node. If the shortest distance is less than the set safety distance, discard the newly generated tree node and resample until a complete collision-free path is obtained for the industrial robot moving from the starting point to the target point. The Euclidean distance calculation formula is as follows:

[0021] Where D ji represents the Euclidean distance between the center of the j-th bounding sphere and the i-th obstacle point cloud, (P sphejx ,P sphejy ,P sphejz ) is the coordinate of the j-th bounding sphere on the industrial robot in the base coordinate system, (P obstix ,P obstiy ,P obstiz ) is the coordinate of the i-th obstacle point cloud in the base coordinate system.

[0022] Preferably, in step S5, the equation of the trajectory curve is as follows:

[0023] Where θ j1 ,θ j2 ,θ j3 , respectively represent the 1st, 2nd, and 3rd motion trajectories of the jth joint of the industrial robot, which respectively adopt cubic polynomial interpolation, quintic polynomial interpolation, and cubic polynomial interpolation, i.e., 3-5-3 polynomial interpolation, j = 1, 2, …, n, where n is the actual number of joints of the industrial robot; t1, t2, and t3 respectively represent the interpolation time of the industrial robot along the 1st, 2nd, and 3rd trajectory segments, a j =[a j10 a j11 a j12 a j13 a j20 a j21 aj22 a j23 a j24 a j25 a j30 a j31 a j32 a j33 ] are polynomial coefficients;

[0024] According to the position-time trajectory curve equation, the angular velocity-time trajectory curve and angular acceleration-time trajectory curve formulas of joint j can be obtained as follows:

[0025] When using 3-5-3 polynomial interpolation to plan a trajectory, you need to set constraints: X ji represents the known interpolated position of the jth joint, i = 0, 1, 2, 3; the starting point of the trajectory is X j0 , the middle path point is X j1 、X j2 The end point of the trajectory is X j3 , set the velocity and acceleration of the initial and end points to 0, and the velocity and acceleration between the path points are continuous;

[0026] Combining the above constraints with the above formula, the relationship between the polynomial coefficients, interpolation position, and interpolation time can be deduced as follows: b=[0 0 0 0 0 0 X j3 0 0 X j0 0 0 X j2 X j1 ] T ;

[0027] a=A- 1 ·b=[a j13 a j12 a j11 a j10 a j25 a j24 a j23 a j22 a j21 a j20 a j33 a j32 a j31 a j30 ] T ;

[0028] It can be seen from the above formula that when the four interpolation positions and three interpolation times are known, the polynomial coefficients can be obtained, and then the specific equations of the position-time trajectory curve, velocity-time trajectory curve, and acceleration-time trajectory curve can be obtained, that is, the trajectory equation of the joint space.

[0029] Preferably, in step S6, the objective function is as follows:

[0030] The constraints are as follows:

[0031] Where, f(t j ) represents the shortest time for the jth joint to move on the 3-5-3 interpolation trajectory, v ji is the angular velocity of the jth joint on the i-th trajectory, a ji is the angular acceleration of the jth joint on the i-th trajectory; v max is the maximum angular velocity of each trajectory constraint, a max is the maximum angular acceleration constrained for each trajectory segment.

[0032] Preferably, in step S7, the improved particle swarm algorithm uses PWLCM chaos mapping to initialize the population on the basis of the basic particle swarm algorithm, and uses an adjustable nonlinear decreasing inertia weight to enable the algorithm to quickly converge to the global optimum; the specific process of the improved particle swarm algorithm is:

[0033] S71. Determine the number of iterations N, the population size M, and the maximum angular velocity v max , maximum angular acceleration a max ;

[0034] S72, use PWLCM chaotic mapping to initialize the particle population, the particle dimension is 3, which contains the particle position and particle flight speed information, the particle position is the time tj1, tj2 of joint j running on the three trajectories j2 , t j3 , t j1 , t j2 , t j3 is an M-dimensional vector, and the particle flying speed is v j1 、v j2 、v j3 , v j1 、v j2 、v j3 is an M-dimensional vector, and the upper limit of particle velocity is v jimax The chaotic mapping can make the initial particles evenly distributed in the solution space, that is, to ensure the randomness and uniformity of the solution, so as to improve the global search ability of the particle swarm algorithm. The PWLCM chaotic mapping is as follows:

[0035] Where j is the jth joint of the industrial robot, i represents the i-th trajectory, m represents the number of the particle, that is, the m-th solution, and p is the control parameter, and the value of p is in the range of (0, 0.5);

[0036] S73, check whether the particles obtained by the chaotic mapping meet the requirements, and set the particle position t j1 , t j2 , t j3 , trajectory starting point X j0 , the middle path point is X j1 、X j2 , the end point is X j3 Substitute the polynomial coefficients, interpolation position, and interpolation time into the relationship between them to obtain the polynomial coefficients, and then obtain the position-time trajectory curve, angular velocity-time trajectory curve, and angular acceleration-time trajectory curve of joint j, and judge whether the velocity and acceleration of each trajectory segment meet the constraint value v max and a max If it is satisfied, it is saved as the initial particle and the fitness f of the particle is calculated. If it is not satisfied, the particle is regenerated for testing until M initial particles are generated to form the initial population;

[0037] S74. Perform iterative optimization on each particle in the particle population. First, update the position and velocity of the particle during the iteration. The update formula is as follows:

[0038] Where, is the d-th dimension component of the flight velocity of the m-th particle at the k-th iteration of joint j, is the d-th dimension component of the position of the m-th particle at the k-th iteration of joint j, r1 and r2 are random numbers between [0,1], p jmd is the d-th dimension component of the optimal position of the m-th particle in joint j, p jgd is the d-th dimension component of the optimal position of the particle swarm at joint j, c1 and c2 are cognitive weight factors, w is the inertia weight, and kl is an adjustable slope coefficient, which can adjust the change mode of w in the iterative process and affect the convergence and global optimality of the particle swarm algorithm;

[0039] Then determine whether the particle speed meets the upper limit v jimax If it is satisfied, it is set as the current velocity of the particle. If it is not satisfied, the current velocity of the particle is set to v jimax, and then calculate the fitness f of the particle and check whether the trajectory curve corresponding to the particle meets the constraints. If the particle meets the constraints and the fitness f is less than the previous generation of particles, it is updated to the new generation of particles. Otherwise, the previous generation of particles is used as the new generation of particles; the fitness f of the new generation of particles is compared with the optimal fitness f of the current population. g In contrast, if the fitness f of the new generation of particles is less than the optimal fitness f of the current population g , then the new generation of particles is regarded as the optimal particle of the population, and the fitness of the new generation of particles is regarded as the optimal fitness of the population f g , the position of the new generation particle is taken as the optimal position of the population t j1g , t j2g , t j3g Repeat the iteration until the number of iterations reaches N, and obtain the position data t of the optimal particle in the population j1g , t j2g , t j3g As the running time of joint j on the three trajectories.

[0040] Preferably, in step S8, since the planning time of each joint is different in the point-to-point joint planning of the industrial robot, in order to ensure the synchronization of the joint movement, the running time of each joint needs to be synchronized to ensure that the end effector of the industrial robot reaches the predetermined position with the cooperation of each joint; the maximum value of the running time of each joint in the first, second and third trajectory segments is taken as the interpolation time of all joints in the trajectory segment, that is, t1=max{t j1g}, t2=max{t j2g}, t3=max{t j3g}(j=1,2,……,n).

[0041] Compared with the prior art, the present invention has the following beneficial effects:

[0042] 1. In this method, the distance between the industrial robot and the obstacle is calculated in a simple and time-saving manner. By using virtual joint interpolation and then completely enveloping the industrial robot with a spherical bounding box at the joints to simplify the model, the distance calculation can be completed by simply calculating the distance between the center of the finite spherical bounding box and each point in the obstacle point cloud.

[0043] 2. This method accelerates the planning of the industrial robot's terminal path, resulting in a shorter and smoother path. The addition of an artificial potential field and target bias mechanisms influences the growth direction of tree nodes, allowing the random tree to grow directionally toward the target point. The adjacent space search and tree reconnection mechanism of the RRT* algorithm reduce inflection points in the path, making the path shorter and smoother.

[0044] 3. The collision detection process is streamlined, preventing the generated complete path from being unusable. Leveraging the G-APF-RRT* algorithm's advantage of having fewer sampling points, the algorithm simultaneously performs sampling and detection, simplifying the process, shortening path planning time, and preventing the planned complete path from being unusable.

[0045] 4. The global search capability of the particle swarm algorithm is improved, making it more likely to find the optimal solution. By using the PWLCM chaotic map to initialize the particle population, the initial particles are evenly distributed in the solution space, ensuring the randomness and uniformity of the solution, thereby improving the global search capability of the particle swarm algorithm.

[0046] 5. The improved particle swarm algorithm can quickly converge to the global optimal solution. By using an adjustable nonlinear decreasing inertia weight, we can adjust the way the inertia weight w changes during the iteration process. At the same time, we ensure that the inertia weight w is large in the early stages of the particle swarm algorithm to improve global search capabilities, and small in the later stages to accelerate convergence. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] FIG1 is a flow chart of an obstacle avoidance method for an industrial robot in a complex environment according to the present invention;

[0048] FIG2 is a simplified diagram of an industrial robot and a point cloud of obstacles in an obstacle avoidance method for an industrial robot in a complex environment according to the present invention;

[0049] FIG3 is a diagram illustrating a camera arrangement for an industrial robot obstacle avoidance method in a complex environment according to the present invention;

[0050] FIG4 is a diagram illustrating a tree node generation process of an RRT* algorithm incorporating an artificial potential field mechanism into an obstacle avoidance method for an industrial robot in a complex environment according to the present invention;

[0051] FIG5 is a schematic diagram of the Euclidean distance Dji of an obstacle avoidance method for an industrial robot in a complex environment according to the present invention;

[0052] FIG6 is a PWLCM chaotic map distribution diagram of an industrial robot obstacle avoidance method in a complex environment according to the present invention;

[0053] FIG7 is a curve diagram showing the change of w for different kl values ​​in an obstacle avoidance method for an industrial robot under a complex environment according to the present invention;

[0054] FIG8 is a ROS industrial robot initial and final pose simulation diagram of an industrial robot obstacle avoidance method in a complex environment according to the present invention;

[0055] FIG9 is a simulation diagram of the obstacle avoidance motion trajectory of an ROS industrial robot in a complex environment according to an obstacle avoidance method for an industrial robot according to the present invention;

[0056] FIG10 is a path planning diagram of the G-APF-RRT* algorithm of an industrial robot obstacle avoidance method in a complex environment according to the present invention. DETAILED DESCRIPTION

[0057] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the implementation regulations described are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0058] Referring to Figures 1 to 10 , the present invention provides a technical solution: a method for industrial robot obstacle avoidance in a complex environment, comprising the following steps:

[0059] Step 1: Simplify the model of the industrial robot in the workspace and simplify the obstacles into obstacle point clouds, as shown in Figure 2. The industrial robot model simplification method is as follows: determine the real joint points of the industrial robot, interpolate virtual joint points at a certain distance on the connecting rods between adjacent real joint points, and then use a spherical bounding box with the joint points as the center to completely envelop the industrial robot body, ensuring that the industrial robot can be completely replaced in any posture.

[0060] Step 2: Place the depth camera outside the motion space of the industrial robot using an "eyes outside the hands" arrangement, as shown in Figure 3. Before path planning, obtain point cloud information of the industrial robot and obstacles in the workspace. Use a point cloud filter to filter out the point cloud of the industrial robot itself, retaining only the point cloud of the obstacles in the workspace.

[0061] Step 3: Before the industrial robot moves, the G-APF-RRT* algorithm is used to plan a feasible path in the workspace. The G-APF-RRT* algorithm is an improved Rapidly Expanding Random Trees algorithm (RRT algorithm for short). The RRT algorithm connects randomly sampled points in the workspace based on a random sampling method to form a feasible path. The connection process is similar to the growth of a tree.

[0062] Specifically, the first improvement is based on the RRT algorithm as the basic framework, on which the adjacent space search and tree reconnection mechanism are introduced, which is called the RRT* algorithm. The adjacent space search means that when a random node q rand After selecting the parent node, the newly generated tree node q new As the center of the circle, take the other nodes in the circular area with a radius of r as candidate parent nodes and calculate the tree nodes q with these candidate parent nodes as the tree nodes newThe parent node is the cost value of the path, if there is a candidate parent node and the tree node q new If the cost of the new path is less than the cost of the original path and the new path does not intersect with obstacles, then the parent node to be selected is set as the tree node q new The parent node of , delete the original path. The tree reconnection mechanism is to determine the new parent node in the adjacent space and use the new tree node q new As a parent node, it connects other nodes in the adjacent space. If there is a node in the adjacent space with the tree node q new If the cost of the new path when it is the parent node is less than the cost of the original path and the new path does not intersect with the obstacle, the new path connection method is adopted.

[0063] The second improvement is to introduce a target bias mechanism into the random sampling process of RRT* to speed up the path search efficiency; by setting the target bias threshold p, the program will generate a random number N within [0,1] before randomly sampling points. rand , when N rand ≤p, the target point q goal As a random node q rand Select new nodes so that the random tree grows towards the target point with a certain probability; when N rand When >p, random points are sampled in the state space to ensure the completeness of the algorithm.

[0064] The third improvement is to introduce the idea of ​​artificial potential field into the RRT* algorithm, that is, the target point generates an attraction and the obstacle generates a repulsion to influence the selection of new tree nodes, as shown in Figure 4. In the process of generating new tree nodes, the random sampling point q rand For the tree node q closest to it nearest Generate a tree node q nearest Point to a random point q rand The unit vector Q rand , and the target point q goal The gravitational force generates a tree node q nearest Point to the target point q goal The unit vector Q rand , the obstacle generates a tree node q nearest A unit vector Q pointing in the opposite direction of the obstacle rep By adjusting the growth step size s, the attraction gain coefficient g, and the repulsion gain coefficient k, the above three vectors are added to obtain a resultant vector Q new , in the resultant vector Q rand Direction takes new tree node q with growth step s new , which can make the new tree node grow stably towards the target point; the resultant vector Q new The formula is:

[0065] In the formula, in the formula, Q repi is the repulsive force unit vector generated by the i-th obstacle, n is the number of obstacles, is a random sampling point q rand The position vector of is the tree node q nearest The position vector of is the target point q goal The position vector of is the position vector of the i-th obstacle, s is the growth step, g is the gravitational gain coefficient, and the new tree node q can be adjusted by changing the size of g new Deflection towards target point q goal The degree of direction, k is the repulsion gain coefficient, and the new tree node q can be adjusted by changing the size of k new The degree of growth in the direction away from the obstacle; r ri is a random sampling point q rand The shortest distance to the i-th obstacle, r0 is the influence distance of the repulsive field generated by the obstacle;

[0066] Step 4: To avoid collisions between the joints of the industrial robot and obstacles when the end of the industrial robot moves along the path, the newly generated tree node q new Perform joint collision detection to obtain a complete collision-free path for the industrial robot to move from the starting point to the target point. The process of joint collision detection is as follows: solve the position information of each joint when the end of the industrial robot is located at the newly generated tree node through inverse kinematics, and calculate the Euclidean distance D between the center of each enclosing ball of the industrial robot and the obstacle point cloud in the base coordinate system. ji , see Figure 5, obtain the shortest distance and compare it with the safety distance. If the shortest distance is greater than the set safety distance, retain the newly generated tree node. If the shortest distance is less than the set safety distance, discard the newly generated tree node and resample until a complete collision-free path is obtained for the industrial robot moving from the starting point to the target point. The Euclidean distance calculation formula is as follows:

[0067] Where D ji represents the Euclidean distance between the center of the j-th bounding sphere and the i-th obstacle point cloud, (P sphejx ,P sphejy ,P sphejz ) is the coordinate of the j-th bounding sphere on the industrial robot in the base coordinate system, (P obstix ,P obstiy ,P obstiz ) is the coordinate of the i-th obstacle point cloud in the base coordinate system;

[0068] Step 5. Perform inverse kinematics calculation on the obtained collision-free complete path to obtain the joint positions of each joint of the industrial robot in the joint space, i.e., the interpolated positions, which are used for subsequent trajectory interpolation. The joint space trajectory of the industrial robot is planned using the 3-5-3 polynomial interpolation method to obtain the joint space position-time trajectory curve, angular velocity-time trajectory curve, and angular acceleration-time trajectory curve of the industrial robot; the trajectory curve equations are as follows:

[0069] Where θ j1 ,θ j2 ,θ j3 , respectively represent the 1st, 2nd, and 3rd motion trajectories of the jth joint of the industrial robot, which respectively adopt cubic polynomial interpolation, quintic polynomial interpolation, and cubic polynomial interpolation, i.e., 3-5-3 polynomial interpolation, j = 1, 2, …, n, where n is the actual number of joints of the industrial robot; t1, t2, and t3 respectively represent the interpolation time of the industrial robot along the 1st, 2nd, and 3rd trajectory segments, a j =[a j10 a j11 a j12 a j13 a j20 a j21 a j22 a j23 a j24 a j25 a j30 a j31 a j32 a j33 ] are polynomial coefficients;

[0070] According to the position-time trajectory curve equation, the angular velocity-time trajectory curve and angular acceleration-time trajectory curve formulas of joint j can be obtained as follows:

[0071] When using 3-5-3 polynomial interpolation to plan a trajectory, you need to set constraints: X ji represents the known interpolated position of the jth joint, i = 0, 1, 2, 3; the starting point of the trajectory is X j0 , the middle path point is X j1 、X j2 The end point of the trajectory is X j3, set the velocity and acceleration of the initial and end points to 0, and the velocity and acceleration between the path points are continuous;

[0072] Combining the above constraints with the above formula, the relationship between the polynomial coefficients, interpolation position, and interpolation time can be deduced as follows: b=[0 0 0 0 0 0 X j3 0 0 X j0 0 0 X j2 X j1 ] T ; a=A- 1 ·b=[a j13 a j12 a j11 a j10 a j25 a j24 a j23 a j22 a j21 a j20 a j33 a j32 a j31 a j30 ] T ;

[0073] From the above formula, we can see that when the four interpolation positions and three interpolation times are known, the polynomial coefficients can be obtained, and then the specific equations of the position-time trajectory curve, velocity-time trajectory curve, and acceleration-time trajectory curve can be obtained, that is, the trajectory equation of the joint space;

[0074] Step 6: Establish the optimization objective function and trajectory constraints based on the joint space trajectory equation to prepare for the subsequent time-optimal trajectory planning; the objective function is as follows:

[0075] The constraints are as follows:

[0076] Where, f(t j ) represents the shortest time for the jth joint to move on the 3-5-3 interpolation trajectory, v ji is the angular velocity of the jth joint on the i-th trajectory, a ji is the angular acceleration of the jth joint on the i-th trajectory; v max is the maximum angular velocity of each trajectory constraint, a max is the maximum angular acceleration constrained by each trajectory segment;

[0077] Step 7: Use the improved particle swarm algorithm to calculate the running time t of the three segments of the 3-5-3 polynomial interpolation trajectory. j1 , t j2 , tj3 Optimize the time for each joint of the industrial robot to run along the trajectory to the shortest possible extent, and satisfy the joint angular velocity v during operation. ji , angular acceleration a ji Does not exceed the constraint value v max and a max ;

[0078] Specifically, the improved particle swarm algorithm uses PWLCM chaos mapping to initialize the population on the basis of the basic particle swarm algorithm, and uses adjustable nonlinear decreasing inertia weight to make the algorithm converge quickly to the global optimum; the specific process of the improved particle swarm algorithm is:

[0079] 71) Determine the number of iterations N, the population size M, and the maximum angular velocity v max , maximum angular acceleration a max ;

[0080] 72) PWLCM chaotic mapping is used to initialize the particle population. The particle dimension is 3, which contains the particle position and particle flight speed information. The particle position is the time t when joint j runs on the three trajectories. j1 , t j2 , t j3 , t j1 , t j2 , t j3 is an M-dimensional vector, and the particle flying speed is v j1 、v j2 、v j3 , v j1 、v j2 、v j3 is an M-dimensional vector, and the upper limit of particle velocity is v jimax The chaotic mapping can make the initial particles evenly distributed in the solution space, that is, to ensure the randomness and uniformity of the solution, so as to improve the global search ability of the particle swarm algorithm. The chaotic mapping layout is shown in Figure 6. The PWLCM chaotic mapping is shown as follows:

[0081] Where j is the jth joint of the industrial robot, i represents the i-th trajectory, m represents the number of the particle, that is, the m-th solution, and p is the control parameter, and the value of p is in the range of (0, 0.5);

[0082] 73) Check whether the particles obtained by chaotic mapping meet the requirements and set the particle position t j1 , t j2 , t j3 , trajectory starting point X j0 , the middle path point is X j1 、X j2 , the end point is X j3Substitute the polynomial coefficients, interpolation position, and interpolation time into the relationship between them to obtain the polynomial coefficients, and then obtain the position-time trajectory curve, angular velocity-time trajectory curve, and angular acceleration-time trajectory curve of joint j, and judge whether the velocity and acceleration of each trajectory segment meet the constraint value v max and a max If it is satisfied, it is saved as the initial particle and the fitness f of the particle is calculated. If it is not satisfied, the particle is regenerated for testing until M initial particles are generated to form the initial population;

[0083] 74) Iterate and optimize each particle in the particle population. First, update the position and velocity of the particle during the iteration. The update formula is as follows:

[0084] Where, is the d-th dimension component of the flight velocity of the m-th particle at the k-th iteration of joint j, is the d-th dimension component of the position of the m-th particle at the k-th iteration of joint j, r1 and r2 are random numbers between [0,1], p jmd is the d-th dimension component of the optimal position of the m-th particle in joint j, p jgd is the d-th dimension component of the optimal position of the particle swarm at joint j, c1 and c2 are cognitive weight factors, w is the inertia weight, and kl is an adjustable slope coefficient, which can adjust the change of w in the iterative process, as shown in Figure 7, affecting the convergence and global optimality of the particle swarm algorithm; then judge whether the particle velocity meets the upper limit v jimax If it is satisfied, it is set as the current velocity of the particle. If it is not satisfied, the current velocity of the particle is set to v jimax , and then calculate the fitness f of the particle and check whether the trajectory curve corresponding to the particle meets the constraints. If the particle meets the constraints and the fitness f is less than the previous generation of particles, it is updated to the new generation of particles. Otherwise, the previous generation of particles is used as the new generation of particles; the fitness f of the new generation of particles is compared with the optimal fitness f of the current population. g In contrast, if the fitness f of the new generation of particles is less than the optimal fitness f of the current population g , then the new generation of particles is regarded as the optimal particle of the population, and the fitness of the new generation of particles is regarded as the optimal fitness of the population f g , the position of the new generation particle is taken as the optimal position of the population t j1g , t j2g , t j3g Repeat the iteration until the number of iterations reaches N, and obtain the position data t of the optimal particle in the population j1g , t j2g , tj3g as the running time of joint j on the three trajectories;

[0085] Step 8. Complete the trajectory optimization of all joints and perform joint synchronization. Since the planning time of each joint is different in the point-to-point joint planning of the industrial robot, in order to ensure the synchronization of joint movement, the running time of each joint needs to be synchronized to ensure that the end effector of the industrial robot reaches the predetermined position with the cooperation of each joint. For the 1st, 2nd, and 3rd trajectory segments, the maximum value of the running time of each joint in the trajectory segment is taken and used as the interpolation time of all joints in the trajectory segment, that is, t1 = max{t j1g}, t2=max{t j2g}, t3=max{t j3g}(j=1,2,……,n);

[0086] Step 9: Use the synchronized interpolation times t1, t2, and t3 to perform 3-5-3 polynomial interpolation on each joint to obtain the optimal time trajectory of each joint.

[0087] Step 10: Send the joint space trajectory to the controller to control the industrial robot to move along the trajectory, avoid obstacles and reach the target point. The simulation results are shown in Figures 8 and 9.

[0088] This method shortens the time required for industrial robots to complete actions in complex environments in two ways. First, in terms of planning time: a feasible path is planned in a shorter time, which shortens the time for the industrial robot to perform path planning, and the convergence speed of the particle swarm algorithm is accelerated, which shortens the time for the industrial robot to perform trajectory planning; second, in terms of motion time: an improved particle swarm algorithm is used for time-optimal trajectory planning, which shortens the motion time required for the industrial robot to complete the action.

[0089] 1. When an industrial robot avoids obstacles, calculating the distance between the robot and the obstacle is a complex and time-consuming operation. By interpolating virtual joint points on the robot's links and then enveloping the robot with a spherical bounding box at the joint points to simplify the robot model, combined with the point cloud representation of the obstacle, subsequent distance calculations between the robot and the obstacle only require calculating the distance between the virtual joint points and each point in the obstacle point cloud. This greatly simplifies the distance calculation and shortens the path planning time for the industrial robot.

[0090] 2. The basic RRT algorithm performs path search through random sampling and gradually expanding a tree structure. It can quickly search for and generate feasible paths, demonstrating rapid convergence. The RRT* algorithm reduces path twists and turns through adjacent space search and tree reconnection, making the path smoother and shorter, thereby shortening the robot's motion time. By incorporating artificial potential fields and target bias mechanisms into the sampling process, the random tree grows in a targeted manner toward the target point, further accelerating the path search. The G-APF-RRT* algorithm possesses all of these advantages, significantly reducing the time it takes for industrial robots to plan their paths and the motion time required to complete their movements.

[0091] 3. Conventional joint obstacle avoidance is to perform joint collision detection on each path point after planning the complete path. If a joint collides at a certain path point, the path point needs to be removed, re-pointed and connected to the original path. This process is complicated and may not generate a feasible path. The present invention is based on the advantage of the G-APF-RRT* algorithm with fewer tree nodes, as shown in Figure 10. It adopts the method of performing collision detection immediately after generating the tree node, which simplifies the joint obstacle avoidance process, shortens the path planning time of the industrial robot, and avoids the unavailability of the planned complete path.

[0092] 4. Using PWLCM chaotic mapping to initialize the particle swarm improves the global search capability of the particle swarm algorithm, making the optimized joint running time t j1g , t j2g , t j3g It is closer to the optimal solution, that is, the time the joint spends running on each trajectory is shorter, which shortens the movement time required for the industrial robot to complete the action.

[0093] 5. The inertia weight of the traditional particle swarm algorithm is modified, and an adjustable nonlinear decreasing inertia weight is used to affect the update of particle positions, so that the algorithm can quickly converge to the global optimum and shorten the time for trajectory planning of industrial robots.

[0094] Although the present invention has been described in detail with reference to the aforementioned embodiments, it is still possible for those skilled in the art to modify the technical solutions described in the aforementioned embodiments, or to make equivalent substitutions for some of the technical features therein. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. An industrial robot obstacle avoidance method in a complex environment, characterized in that: The following steps are involved: S1, simplify the model of the industrial robot in the workspace and simplify the obstacles into obstacle point clouds; S2, using the "eyes outside the hands" arrangement to place the depth camera outside the motion space of the industrial robot; S3, using the G-APF-RRT* algorithm to plan a feasible path in the workspace before the industrial robot moves; S4. To avoid collisions between the joints of the industrial robot and obstacles when the end of the industrial robot moves along the path, the newly generated tree node q new Perform joint collision detection to obtain a complete collision-free path for the industrial robot to move from the starting point to the target point; S5. Perform inverse kinematics calculation on the obtained collision-free complete path to obtain the joint positions of each joint of the industrial robot in the joint space, i.e., the interpolated positions, which are used for subsequent trajectory interpolation. The joint space trajectory of the industrial robot is planned by the 3-5-3 polynomial interpolation method to obtain the joint space position-time trajectory curve, angular velocity-time trajectory curve, and angular acceleration-time trajectory curve of the industrial robot; S6. Establish optimization objective function and trajectory constraint conditions according to joint space trajectory equation to prepare for subsequent time optimal trajectory planning; S7, using the improved particle swarm algorithm, the running time t of the three trajectories in the 3-5-3 polynomial interpolation trajectory j1 ,t j2 ,t j3 Optimize the robot so that each joint of the industrial robot can run along the trajectory in the shortest time and meet the joint angular velocity v during operation. ji , angular acceleration a ji Does not exceed the constraint value v max and a max ; S8, complete the trajectory optimization of all joints and perform joint synchronization processing; S9, using the interpolation times t1, t2, t3 after synchronization processing to perform 3-5-3 polynomial interpolation on each joint to obtain the time optimal trajectory of each joint; S10, send the joint space trajectory to the controller, control the industrial robot to move along the trajectory, and complete Avoid obstacles and reach the target point.

2. The obstacle avoidance method for an industrial robot in a complex environment according to claim 1, characterized in that: In step S1, the industrial robot model simplification method is: determine the real joint points of the industrial robot, interpolate virtual joint points at a certain distance on the connecting rods between adjacent real joint points, and then use a spherical bounding box with the joint points as the center of the sphere to completely envelop the industrial robot body, ensuring that the industrial robot can be completely replaced when it is in any position.

3. The obstacle avoidance method for an industrial robot in a complex environment according to claim 1, characterized in that: In step S2, before path planning, point cloud information of the industrial robot and obstacles in the workspace is obtained, and the point cloud of the industrial robot body is filtered out using a point cloud filter, leaving only the point cloud of the obstacles in the workspace.

4. The obstacle avoidance method for an industrial robot in a complex environment according to claim 1, characterized in that: In step S3, the G-APF-RRT* algorithm is an improved rapidly expanding random tree algorithm, which is referred to as the RRT algorithm. The rapidly expanding random tree algorithm connects random sampling points based on a random sampling method in a workspace to form a feasible path, and its connection process is similar to the growth of a tree.

5. The obstacle avoidance method for an industrial robot in a complex environment according to claim 1, characterized in that: In step S4, the process of joint collision detection is as follows: the position information of each joint when the end of the industrial robot is located at the newly generated tree node is solved by inverse kinematics, and the Euclidean distance D between the center of each enclosing ball of the industrial robot and the obstacle point cloud is calculated in the base coordinate system. ji , obtain the shortest distance and compare it with the safety distance. If the shortest distance is greater than the set safety distance, retain the newly generated tree node. If the shortest distance is less than the set safety distance, discard the newly generated tree node and resample until a complete collision-free path is obtained for the industrial robot moving from the starting point to the target point. The Euclidean distance calculation formula is as follows: Where D ji represents the Euclidean distance between the center of the jth bounding sphere and the i-th obstacle point cloud, (P sphejx ,P sphejy ,P sphejz ) is the coordinate of the jth bounding sphere on the industrial robot in the base coordinate system, (P obstix ,P obstiy ,P obstiz ) is the coordinate of the i-th obstacle point cloud in the base coordinate system.

6. The method for avoiding obstacles of an industrial robot in a complex environment according to claim 1, characterized in that: In step S5, the equation of the trajectory curve is as follows: In the formula, θ j1 ,θ j2 ,θ j3 , respectively represent the 1st, 2nd, and 3rd motion trajectories of the jth joint of the industrial robot, which respectively adopt third-order polynomial interpolation, fifth-order polynomial interpolation, and third-order polynomial interpolation, i.e., 3-5-3 polynomial interpolation, j = 1, 2, …, n, where n is the actual number of joints of the industrial robot; t1, t2, and t3 respectively represent the interpolation time of the industrial robot along the 1st, 2nd, and 3rd trajectory, a j =[a j10 a j11 a j12 a j13 a j20 a j21 a j22 a j23 a j24 a j25 a j30 a j31 a j32 a j33 ] are polynomial coefficients; According to the position-time trajectory curve equation, the angular velocity-time trajectory curve and angular acceleration-time trajectory curve formulas of joint j can be obtained as follows: When using 3-5-3 polynomial interpolation to plan a trajectory, you need to set constraints: X ji represents the known interpolation position of the jth joint, i = 0, 1, 2, 3; the starting point of the trajectory is X j0 , the middle path point is X j1 , X j2 The end point of the trajectory is X j3 , set the speed and acceleration of the initial point and the end point to 0, and the speed and acceleration between the path points are continuous; Combining the above constraints with the above formula, the relationship between the polynomial coefficients, interpolation position, and interpolation time can be deduced as follows: b=[0 0 0 0 0 0 X j3 0 0 X j0 0 0 X j2 X j1 ] T ; a=A -1 ·b=[a j13 a j12 a j11 a j10 a j25 a j24 a j23 a j22 a j21 a j20 a j33 a j32 a j31 a j30 ] T ; It can be seen from the above formula that when 4 interpolation positions and 3 interpolation times are known, the polynomial coefficients can be obtained, and then the specific equations of the position-time trajectory curve, velocity-time trajectory curve, and acceleration-time trajectory curve can be obtained, that is, the trajectory equation of the joint space.

7. The obstacle avoidance method for industrial robots in complex environments according to claim 1, characterized in that: In step S6, the objective function is as follows: The constraints are as follows: In the formula, f(t j ) represents the shortest time for the jth joint to move on the 3-5-3 interpolation trajectory, vji is the angular velocity of the jth joint on the i-th trajectory, a ji is the angular acceleration of the jth joint on the i-th trajectory; v max is the maximum angular velocity constrained by each trajectory segment, a max is the maximum angular acceleration constrained by each trajectory segment.

8. The method for avoiding obstacles of an industrial robot in a complex environment according to claim 6, characterized in that: In step S7, the improved particle swarm algorithm uses PWLCM chaotic mapping to initialize the population on the basis of the basic particle swarm algorithm, and uses an adjustable nonlinear decreasing inertia weight to make the algorithm converge quickly to the global optimum; the specific process of the improved particle swarm algorithm is: S71, determine the number of iterations N, the population size M, and the maximum angular velocity v max , maximum angular acceleration a max ; S72, use PWLCM chaotic mapping to initialize the particle population, the particle dimension is 3, which contains the particle position and particle flight speed information, the particle position is the time t that joint j runs on the three trajectories j1 , t j2 ,t j3 , t j1 ,t j2 ,t j3 is an M-dimensional vector, and the particle flying speed is v j1 、v j2 、v j3 , v j1 、v j2 、v j3 is an M-dimensional vector, and the upper limit of particle speed is v jimax ; The chaotic mapping can make the initial particles evenly distributed in the solution space, that is, to ensure the randomness and uniformity of the solution, so as to improve the global search ability of the particle swarm algorithm. The PWLCM chaotic mapping is as follows: Where j is the jth joint of the industrial robot, i represents the i-th trajectory, m represents the number of the particle, i.e. the m-th solution, and p is the control parameter, and the value of p is in the interval (0, 0.5); S73, check whether the particles obtained by the chaotic mapping meet the requirements, and set the particle positions t j1 ,t j2 ,t j3 , trajectory starting point X j0 , the middle path point is X j1 , X j2 , the end point is X j3 Substitute the polynomial coefficients, interpolation position, and interpolation time into the relationship between them to obtain the polynomial coefficients, and then obtain the position-time trajectory curve, angular velocity-time trajectory curve, and angular acceleration-time trajectory curve of joint j to determine whether the velocity and acceleration of each segment of the trajectory meet the constraint value v max and a max If it is satisfied, it is saved as the initial particle and the fitness f of the particle is calculated. If it is not satisfied, the particle is regenerated for testing until M initial particles are generated to form the initial population. S74. Iterate and optimize each particle in the particle population. First, update the position and velocity of the particle during the iteration. The update formula is as follows: In the formula, is the d-th dimension component of the flight velocity of the m-th particle at the k-th iteration of joint j, is the d-th dimension component of the position of the m-th particle at the k-th iteration of joint j, r1 and r2 are random numbers between [0,1], p jmd is the d-th dimension component of the optimal position of the m-th particle in joint j, p jgd is the d-th dimension component of the optimal position of the particle population at joint j, c1 and c2 are cognitive weight factors, w is the inertia weight, and kl is an adjustable slope coefficient, which can adjust the change mode of w in the iteration process and affect the convergence and global optimality of the particle swarm algorithm; Then determine whether the particle speed meets the upper limit v jimax If it is satisfied, it is set as the current velocity of the particle. If it is not satisfied, the current velocity of the particle is set to v jimax , and then calculate the fitness f of the particle and check whether the trajectory curve corresponding to the particle meets the constraints. If the particle meets the constraints and the fitness f is less than the previous generation of particles, it is updated to a new generation of particles, otherwise the previous generation of particles is used as the new generation of particles; the fitness f of the new generation of particles is compared with the optimal fitness f of the current population g In contrast, if the fitness f of the new generation of particles is less than the optimal fitness f of the current population g , then the new generation of particles is regarded as the optimal particle of the population, and the fitness of the new generation of particles is regarded as the optimal fitness of the population f g , the position of the new generation of particles is taken as the optimal position of the population t j1g ,t j2g ,t j3g Repeat the iteration until the number of iterations reaches N, and obtain the position data t of the optimal particle in the population j1g ,t j2g ,t j3g As the running time of joint j on the three trajectories.

9. The method for avoiding obstacles of an industrial robot in a complex environment according to claim 1, characterized in that: In step S8, since the planning time of each joint is different in the point-to-point joint planning of the industrial robot, in order to ensure the synchronization of joint movement, the running time of each joint needs to be synchronized to ensure that the end effector of the industrial robot reaches the predetermined position with the cooperation of each joint; the maximum value of the running time of each joint in the first, second, and third trajectory segments is taken as the interpolation time of all joints in the trajectory segment, that is, t1=max{t j1g }, t2=max{t j2g }, t3=max{t j3g }(j=1,2,……,n).

Images