Obstacle avoidance path planning method and device based on APF hot-line work robot and medium

Abstract

The invention relates to the field of robot path planning, and discloses an APF-based hot-line work robot obstacle avoidance path planning method and device and a medium, and the method comprises the steps: carrying out the kinematics modeling of a six-degree-of-freedom mechanical arm through a traditional D-H parameter method, determining the coordinate system conversion relation between the adjacent joints of the mechanical arm, and obtaining the tail end pose of the mechanical arm; the radius of the connecting rod of the mechanical arm is fused into enveloping of the surrounding ball and the cylinder, and a collision detection model is designed and used for detecting whether collision occurs or not; an improved APF-SA fusion algorithm is applied, a starting point, a target point and an obstacle are input, and a planned path is obtained; according to the invention, the problem that a traditional artificial potential field method cannot reach a target and is easy to fall into a local optimal value is solved; the method can adapt to different environments, and compared with a traditional APF algorithm, the planning time is shortened, and the planning success rate is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot path planning, and in particular to an obstacle avoidance path planning method, device and medium based on an APF live working robot. Background Art

[0002] With the rapid development of the power industry and the increasing demand for efficient automation systems, more and more live working robots are being used for power inspection and transmission line maintenance, replacing manual work to complete special operations in high-precision or harsh environments, thereby improving the operational safety of power distribution and reducing the labor intensity of workers.

[0003] The core of the live-line working robot lies in the control of the robotic arm. In the research of the robotic arm, the path planning problem is crucial. It requires designing an effective and safe trajectory for the robotic arm so that it can navigate from the initial position to the target position while bypassing various obstacles. In a complex distribution network environment, whether the robotic arm can quickly plan a short and smooth path is the key to the live-line working robot completing its task.

[0004] The traditional path planning method (APF) currently used has two main problems in the path planning of the robot arm of the live working robot. First, when the robot arm approaches the target point, the repulsion generated by the obstacles may cause the target to be unreachable, that is, the robot arm cannot reach the target point smoothly. Second, it is easy to fall into the local minimum, resulting in the robot arm being unable to find the global optimal path in a complex environment. These problems are caused by the fixed characteristics of the traditional APF repulsion function and the lack of global search capabilities, which limits its application effect in complex distribution network environments. Summary of the invention

[0005] The purpose of the present invention is to propose an obstacle avoidance path planning method for an APF live working robot, so as to solve the technical problems of target unreachability and falling into local minimum that are easy to occur in the complex environment of live working robots in distribution networks due to the current traditional obstacle avoidance path planning methods.

[0006] Specifically, the present invention provides an obstacle avoidance path planning method based on an APF live working robot, comprising the following steps:

[0007] S1. Use the traditional DH parameter method to perform kinematic modeling on the 6-DOF robotic arm, determine the relationship between the coordinate system transformations of each adjacent joint of the robotic arm, and obtain the end position of the robotic arm;

[0008] S2. Integrate the radius of the robot arm link into the enveloping sphere and cylindrical envelope, and design a collision detection model to detect whether a collision occurs;

[0009] S3. Apply the improved APF-SA fusion algorithm, input the starting point, target point, and obstacles, and obtain the planned path.

[0010] A storage medium stores instructions and data for implementing an obstacle avoidance path planning method for an APF live working robot.

[0011] An obstacle avoidance path planning device based on an APF live working robot comprises: a processor and the storage medium; the processor loads and executes instructions and data in the storage medium to implement an obstacle avoidance path planning method based on an APF live working robot.

[0012] The beneficial effects provided by the present invention are:

[0013] 1. Solved the problem that the traditional artificial potential field method is unattainable and easy to fall into the local optimal value;

[0014] 2. The improved APF-SA fusion algorithm can adapt to different environments, and compared with the traditional APF algorithm, it shortens the planning time and improves the planning success rate;

[0015] 3. By building a distribution network live operation working environment model for verification, the improved algorithm is feasible in the actual distribution network live operation working environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is a schematic diagram of the process of the method of the present invention;

[0017] Figure 2 It is the obstacle avoidance path planning hardware system for live working robots;

[0018] Figure 3 It is a schematic diagram of the collision model;

[0019] Figure 4 There are two different collision detection models;

[0020] Figure 5 It is a flow chart of the improved algorithm of the present invention;

[0021] Figure 6 This is a graph showing the experimental results of improving the adaptability of APF-SA in different environments;

[0022] Figure 7 It is a schematic diagram of the working of the hardware device of an embodiment of the present invention. DETAILED DESCRIPTION

[0023] To make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described below with reference to the accompanying drawings.

[0024] Before formally describing the present invention, the scheme of the present invention is first generally described for easy understanding.

[0025] Please refer to Figure 1 The present invention provides an obstacle avoidance path planning method based on an APF live working robot, comprising:

[0026] S1. Use the traditional DH parameter method to perform kinematic modeling on the 6-DOF robotic arm, determine the relationship between the coordinate system transformations of each adjacent joint of the robotic arm, and obtain the end position of the robotic arm;

[0027] It should be noted that the obstacle avoidance path planning hardware system of the live working robot used in the present invention is as follows: Figure 2 shown.

[0028] The object is the UR10 robot arm with 6 degrees of freedom used by the live working robot of the distribution network.

[0029] The DH parameter method of the present invention performs kinematic modeling on the 6-DOF robotic arm, because only the joint angles of the UR10 robotic arm are variable, while the other three parameters are fixed values.

[0030] The parameters of the DH model are shown in Table 1.

[0031] Table 1 UR10 robot model parameters

[0032]

[0033] According to the parameter table of each link coordinate system of the robot and the DH model, the homogeneous transformation matrix between the link coordinate systems can be derived:

[0034]

[0035] In formula (1): a i For along X i Axis, from Z i Axis moves to Z i+1 The distance, α i For along X i Axis, from Z i Axis moves to Z i+1 The angle, θ i For along Z i Axis, from X i-1 Axis moves to X i The angle, d i For along Z i Axis, from X i-1 Axis moves to X i The distance sθ i represents sin(θ i ), cθ irepresents cos(θ i ), sα i represents sin(α i ), cα i represents cos(α i ).

[0036] By substituting the DH parameters given in Table 1 into formula (1), the end position of the robot can be solved by multiplying the transformation matrix of each link:

[0037]

[0038] Where: n x 、n y 、n z , o x , o y , o z 、a x 、a y 、a z is the end posture component of the robot arm; p x 、p y 、p z is the end position component of the robot arm.

[0039] S2. Integrate the radius of the robot arm link into the enveloping sphere and cylindrical envelope, and design a collision detection model to detect whether a collision occurs;

[0040] It should be noted that, for the UR10 robot arm studied in this paper, the collision model of the robot arm is constructed by using the hierarchical envelope box method, and the connecting rods of the first three joints are enveloped by cylinders, and the last three joints are enveloped by spheres. Figure 3 shown.

[0041] The collision model of obstacles is constructed by using the bounding sphere method and the cylindrical envelope method. To further simplify the collision model, the radius of the robot arm link is integrated into the bounding sphere and the cylindrical envelope. Figure 4 shown.

[0042] First, let’s deal with the case of the bounding sphere (b). Let the radius of the connecting rod be r1 and the radius of the bounding sphere be r2. Then the radius of the bounding sphere after integration is r = r 1+ r2. Assume that the points at both ends of the center line are (x1, y1, z1) and (x1, y1, z1), respectively. Then the equation of the line where the center line lies is shown in equation (3).

[0043]

[0044] Let the vertical coordinate from the center point of the enclosing sphere to the center line of the robot arm connecting rod be c1(x c ,y c ,zc ), the coordinates of the center of the sphere are c0(x0,y0,z0), then the straight line where c1c0 is located is perpendicular to the center line, and equation (4) can be obtained.

[0045]

[0046] The coordinates of the vertical point c1 can be calculated by using equations (3) and (4), assuming that the distance between the center of the circle and the center line of each connecting rod is d i ,but:

[0047]

[0048] When i When ≤r, the robot arm collides with the obstacle, otherwise no collision occurs.

[0049] For the case of cylindrical envelope (a), the center line segment of the cylindrical obstacle is divided into a set of points with a minimum step size h N is the number of points in the set. Repeat the above process and calculate all the points according to formulas (3), (4), and (5). The distance to the center line of each connecting rod and determine whether a collision occurs.

[0050] S3. Apply the improved APF-SA fusion algorithm, input the starting point, target point, and obstacles, and obtain the planned path.

[0051] It should be noted that there are two problems in the practical application of the traditional artificial potential field method: one is the problem of unreachable targets, and the other is the problem of local optimal solutions.

[0052] 1) To address the unreachable target problem, it is proposed to introduce the relative distance between the end point of the manipulator and the target point and the repulsion adjustment factor into the repulsion potential function to change the repulsion effect of obstacles near the target point. When the distance to the target point is closer, the repulsion of the obstacles on the manipulator is smaller, so that the end point of the manipulator can reach the target point smoothly. The improved repulsion potential function is shown in formula (6):

[0053]

[0054] Where: U rep represents the repulsive potential function; K r is the repulsive factor; d is the distance between the robot and the obstacle; d0 is the influence range of the repulsive potential field of the obstacle; P r is the position coordinate of the end of the robot arm; P g is the target point position coordinate; a is the improvement factor, d g is the relative distance between the end of the robot arm and the target point.

[0055] The corresponding repulsive force F rep for:

[0056]

[0057] where F rep1 The direction is from the obstacle to the car, F rep2 The direction of the car points to the target point.

[0058] The gravitational potential field function model of the target point is as follows:

[0059]

[0060] Where: U att represents the gravitational potential function; K a Represents the gravitational factor: P r is the position coordinate of the end of the robot arm; P g is the target point position coordinate. It should be noted that P r Corresponding to x j , P r It is the general form that refers to the current position of the end of the robot arm, x j It is the current position of the end of the robot arm during the path planning process.

[0061] The corresponding gravitational force F att for:

[0062]

[0063] The resultant potential field on the robotic arm is:

[0064] U total =U att +U rep (10)

[0065] The resultant force on the robot arm is:

[0066]

[0067] Where: m is the number of obstacles affecting the robotic arm.

[0068] 2) The traditional simulated annealing algorithm is prone to stagnation at the local minimum solution during the search process in complex scenarios and requires multiple solutions at the same temperature, resulting in low planning efficiency.

[0069] The present invention proposes an adaptive temperature increase strategy to improve the simulated annealing algorithm. When the algorithm falls into a local minimum solution, the temperature of the algorithm operation is artificially increased to increase the probability of the algorithm accepting a poor solution, thereby making it more likely to jump out of the local optimal solution.

[0070] If there is no feasible solution better than this solution in the neighborhood of the feasible solution generated during the algorithm operation, that is:

[0071] max{fk-L / 100 ,…,f k-1 ,f k+1 ,…,f k+L / 100}≤f k (12)

[0072] It is defined as the algorithm has fallen into a local optimum, where k represents the number of iterations at the current temperature when the algorithm falls into the local optimum, and L represents the length of the algorithm chain.

[0073] If the temperature increase is too small during the local heating operation, the acceptance probability of the poor solution will be limited, and the local optimal solution cannot be jumped out; if the temperature increase is too large, the algorithm's acceptance probability of the poor solution is close to 100%, and the algorithm optimization will enter a long global search, which seriously reduces the convergence speed of the algorithm. Therefore, the concept of solution set diversity is established to design an adaptive heating strategy. The feasible solutions generated during the operation of the algorithm form a solution set, and the real-time optimal fitness value f of the solution set recorded by the algorithm is used. max The real-time average fitness value f of the solution set avg The relationship between the current solution set diversity F β :

[0074]

[0075] When f avg The value gradually approaches f max When , it indicates that the diversity of the solution set is reduced. The lower the population diversity, the greater the possibility that the algorithm will fall into the local optimum, and the greater the temperature rise should be. In addition, according to the local optimal solution and f max The fitness value relationship between them is used to divide the local optimal solution into three levels. The closer the fitness value of the local optimal solution is to f max , the smaller the temperature increase of the algorithm, therefore, the adaptively increased temperature ΔT is:

[0076]

[0077] Where T is the current temperature of the algorithm, ΔT min =0.3T.

[0078] In general, the overall flow chart of the improved algorithm of the present invention is as follows: Figure 5 As shown:

[0079] The present invention first adopts an improved artificial potential field method for path planning. When it falls into a local minimum, it adopts an improved simulated annealing algorithm for path planning. When it jumps out of the local minimum, it switches back to the artificial potential field method for path planning. The two methods jointly complete the path planning task.

[0080] The specific process is as follows:

[0081] (1) Set the starting point and end point of the robot end and the position of the obstacle in the map. Let the current point of the robot end be S and the local minimum point be X.

[0082] (2) Determine whether the end of the robot arm has fallen into a local minimum solution. If it has fallen into a local minimum solution, start the simulated annealing strategy and set the initial temperature T = T0.

[0083] (3) Generate a random point x near the current point j , where x j =x+Δx, Δx is a uniformly distributed minimal random perturbation (Δx is a random point near point x, and its distance from x is step size λ).

[0084] (4) Calculate the potential field strength U(x) at the current point according to the potential field function formula j ) and the difference between the potential field strength U(x) in the initial state ΔU=U(x j )-U(x).

[0085] (5) If ΔU≤0, then x j Set as next starting point.

[0086] (6) If ΔU>0, then with probability P, j Set as the next starting point, where K is the Boltzmann constant.

[0087] (7) If it is determined that the robot arm has jumped out of the local minimum point, the algorithm stops and moves to a new position. Otherwise, continue to evaluate whether the solution obtained by the simulated annealing algorithm at the current temperature falls into the local minimum. If it does, record the solution set f avg and f max , and increase the temperature T according to formula (14); if it does not fall into, reduce the temperature T.

[0088] (8) Return to step (2) and repeat the above steps until the system reaches equilibrium. Usually, T(t) = αT(t-1), where 0.85≤α≤1, α represents the annealing rate.

[0089] (9) The higher the initial temperature of the robot arm, the better. The higher the initial temperature, the stronger the global exploration ability and the more likely it is to find the global optimal solution. In step (9), select a suitable annealing rate, which is related to the distance between the robot arm and the target point. The closer the distance, the faster the annealing rate can be defined. In step (5), set a random probability P a When P>P a When x=x j ; When P≤P a , it indicates that the random point has failed and needs to be reset.

[0090] During the algorithm operation, U(x j )≤U(x) is not used as the criterion for judging whether the robot arm jumps out of the local minimum. Instead, the distance from the local minimum point to the current point needs to be calculated. This value is set to 6 times the step size. If the distance from the local minimum point to the current point is greater than the set value, the program determines that the robot arm has jumped out of the local minimum point. When the robot arm is at the local minimum point, the simulated annealing method starts to run, and whether it is at the minimum point is determined by whether the resultant force on the robot arm in the potential field is 0 and by detecting the distance.

[0091] The local minimum judgment criteria used here are as follows: if one of the following two conditions is met, the robot path search is determined to be trapped in the local minimum: Condition 1: If the robot is at the node of the path search according to the formula calculated at the node, the resultant external force F total When (x) is 0, the robot is in the local minimum state, that is:

[0092] F total (x) = 0 (18)

[0093] Condition 2: Record the position coordinates of the robot at the node of the path search when the search steps are t1 and t2, that is, X t1 =[x t1 y t1 z t1 ]、X t2 =[x t2 y t2 z t2 ]. If the position coordinate X of the node at step t1 t1 The position coordinate X of the node at step t2 t2 The distance is less than the given minimum detection radius R. At this time, the robot is judged to be in the local minimum state, that is: ||X t1 -X t2 ||≤R(19).

[0094] The present invention provides a specific embodiment as follows.

[0095] Embodiment 1:

[0096] In order to verify the effectiveness of the improved algorithm in this paper, the left arm of the live working robot in the distribution network was taken as the research object, and the algorithm was simulated and verified in MATLAB.

[0097] Simulation 1 verifies the adaptability of the algorithm in different working environments of the distribution network robot. Three experimental scenarios were simulated: Environment 1: horizontal three-phase transmission line, no tree branch obstacles; Environment 2: horizontal three-phase transmission line, with tree branch obstacles; Environment 3: vertical three-phase transmission line, no tree branch obstacles. After three experiments, the results ( Figure 6 ) confirmed that in different environments, the improved artificial potential field method and the improved simulated annealing fusion algorithm can enable the robot to successfully plan a collision-free path, and the path is relatively smooth without mutation points. Simulation 2 Under the same motion control parameters and experimental environment, 10 experiments were conducted on the improved APF-SA fusion algorithm and the traditional artificial potential field method, and the average values ​​were taken. The results are shown in Table 2. It can be seen that the improved APF-SA fusion algorithm has a better effect.

[0098] Table 2 Simulation 2 data comparison

[0099]

[0100] Embodiment 2:

[0101] See also Figure 7 , Figure 7 A schematic diagram of the working of the hardware device of an embodiment of the present invention, wherein the hardware device specifically comprises: an obstacle avoidance path planning device 401 based on an APF live working robot, a processor 402 and a storage medium 403.

[0102] An obstacle avoidance path planning device 401 based on an APF live-line working robot: The obstacle avoidance path planning device 401 based on an APF live-line working robot implements the obstacle avoidance path planning method based on an APF live-line working robot.

[0103] Processor 402: The processor 402 loads and executes the instructions and data in the storage medium 403 to implement the obstacle avoidance path planning method based on the APF live working robot.

[0104] Storage medium 403: The storage medium 403 stores instructions and data; the storage medium 403 is used to implement the obstacle avoidance path planning method based on the APF live working robot.

[0105] The beneficial effects of the present invention are:

[0106] 1. Solved the problem that the traditional artificial potential field method is unattainable and easy to fall into the local optimal value;

[0107] 2. The improved APF-SA fusion algorithm can adapt to different environments, and compared with the traditional APF algorithm, it shortens the planning time and improves the planning success rate;

[0108] 3. By building a distribution network live operation working environment model for verification, the improved algorithm is feasible in the actual distribution network live operation working environment.

[0109] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for obstacle avoidance path planning of an APF live working robot, characterized in that: include: S1. Use the traditional DH parameter method to perform kinematic modeling on the 6-DOF robotic arm, determine the relationship between the coordinate system transformations of each adjacent joint of the robotic arm, and obtain the end position of the robotic arm; S2. Integrate the radius of the robot arm link into the enveloping sphere and cylindrical envelope, and design a collision detection model to detect whether a collision occurs; S3. Apply the improved APF-SA fusion algorithm, input the starting point, target point, and obstacles, and obtain the planned path.

2. The obstacle avoidance path planning method based on the APF live working robot according to claim 1, characterized in that: The end position of the robot arm in step S1 is as follows: In formula (1): n x 、n y 、n z , o x , o y , o z 、a x 、a y 、a z is the end posture component of the robot arm; p x 、p y 、p z is the end position component of the robot arm.

3. The obstacle avoidance path planning method based on the APF live working robot according to claim 1, characterized in that: In step S2, the collision model of the obstacle is constructed using the bounding sphere method, as follows: S21, dealing with the case of the enclosing sphere, assuming that the radius of the connecting rod is r1 and the radius of the enclosing sphere is r2, then the radius of the enclosing sphere after integration is r = r 1+ r2; S22. Assume that the points at both ends of the center line are (x1, y1, z1) and (x1, y1, z1) respectively, then the equation of the line where the center line lies is as shown in formula (2): S23, let the vertical coordinate from the center point of the enclosing sphere to the center line of the robot arm connecting rod be c1(x c ,y c ,z c ), the coordinates of the sphere center are c0(x0,y0,z0), then the line where c1c0 is located is perpendicular to the center line, and equation (3) is obtained: The coordinates of the vertical point c1 can be calculated by using equations (2) and (3), assuming that the distance between the center of the circle and the center line of each connecting rod is d i ,but: When i When ≤r, the robot arm collides with the obstacle, otherwise no collision occurs.

4. The obstacle avoidance path planning method based on the APF live working robot according to claim 3, characterized in that: In step S2, the collision model of the obstacle is constructed using the cylindrical envelope method, as follows: Divide the center line segment of the cylindrical obstacle into a set of points with a minimum step length h N is the number of points in the set. Repeat the above process and calculate all the points according to formulas (2), (3), and (4). The distance to the center line of each connecting rod and determine whether a collision occurs.

5. The obstacle avoidance path planning method based on the APF live working robot according to claim 1, characterized in that: The processing process of improving the APF-SA fusion algorithm in step S3 is as follows: S31, set the starting point and end point of the end of the robot arm and the position of the obstacle in the map; set the current point of the end of the robot arm to S and the local minimum point to X; S32, judging whether the end of the robot arm has fallen into a local minimum solution; if it has fallen into a local minimum solution, starting the simulated annealing strategy and setting the initial temperature T=T0; S33. Generate a random point x near the current point j , where x j =x+Δx, Δx is a uniformly distributed minimal random disturbance; S34, calculate the potential field strength U(x) at the current point according to the potential field function formula j ) and the difference between the potential field strength U(x) in the initial state ΔU=U(x j )-u(x); S35. If ΔU≤0, then x j Set as the next starting point; S36. If ΔU>0, then with probability P, j Set as the next starting point, where K is the Boltzmann constant; S37, if it is determined that the robot arm has jumped out of the local minimum point, the algorithm stops and moves to a new position, otherwise, continue to evaluate whether the solution obtained by the simulated annealing algorithm at the current temperature falls into the local minimum; if it does, record the solution set f avg and f max The corresponding value of f is obtained, and the temperature t is increased according to formula (6); if it does not fall into the trap, the temperature T is reduced; where f max Represents the optimal fitness value and f avg Indicates the real-time average fitness value; S38. Return to step S32 and repeat the above steps until a balanced state is reached.

6. The obstacle avoidance path planning method based on the APF live working robot according to claim 5, characterized in that: The potential field function formula in step S34 is as follows: Among them I att represents the gravitational potential function; K a Represents the gravitational factor: P r is the position coordinate of the end of the robot arm; P g is the target point location coordinate.

7. The obstacle avoidance path planning method based on the APF live working robot according to claim 1, characterized in that: The formula (6) in step S37 is as follows: in, ΔT min =0.3T, T is the current temperature of the algorithm, 8. A storage medium, characterized in that: The storage medium stores instructions and data for implementing an obstacle avoidance path planning method for an APF live working robot as described in any one of claims 1 to 7.

9. An obstacle avoidance path planning device based on an APF live working robot, characterized in that: include: Processor and storage medium; the processor loads and executes instructions and data in the storage medium to implement an obstacle avoidance path planning method based on an APF live working robot as described in any one of claims 1 to 7.

Images