Improved artificial potential field based dual-directional obstacle avoidance path planning method for transporting robot

Abstract

Based on the improved artificial potential field method, a two-way planning method of obstacle avoidance path for transporting robot is proposed. In order to overcome the shortcomings of transporting robot often falling into local optimal solution when planning obstacle avoidance path in complex environment, a two-way planning strategy of obstacle avoidance path based on improved artificial potential field method is designed. First, the reachable midpoint pose is found as the starting point of planning from the start pose to the midpoint pose and the end point of planning from the target pose to the midpoint pose. Then, the improved artificial potential field method is used to plan the obstacle avoidance path. The probability of falling into local optimal solution is reduced. Even if it falls into local optimal solution in complex environment, it also provides a method to quickly jump out of local optimal solution. The problem of target pose being unreachable caused by stagnation or oscillation near obstacles due to falling into local optimal solution is effectively overcome, and the planning path efficiency and accuracy are improved.

Description

Technical Field

[0001] This invention relates to the field of path planning for transport robots, specifically a bidirectional path planning method for obstacle avoidance of transport robots based on an improved artificial potential field method. Background Technology

[0002] Path planning is not only the control foundation for ensuring the autonomous movement and safe operation of handling robots, but also a key technology determining their work efficiency, motion smoothness, and energy consumption. Path planning for handling robots refers to the automatic planning of a collision-free working path in Cartesian space or joint space given the initial and target poses of the handling task. The artificial potential field method is a commonly used path planning algorithm. Its principle is to establish an artificial potential field around the handling robot. The target pose generates an attractive force on the robot's joints, while obstacles generate a repulsive force. By calculating the resultant force through gradient descent, the robot's motion in the environment is designed as motion within an abstract gravitational field, finally reaching the target pose under the action of the resultant force. The advantages of the traditional artificial potential field method are its simplicity, ease of implementation, good real-time performance, and typically smooth paths. The disadvantage is that it may get trapped in local optima, especially in complex environments, easily stagnating or oscillating after getting stuck in a local optimum, leading to situations where the target pose becomes unreachable.

[0003] The existing technology is as follows:

[0004] 1. Technical comparison with "A robotic arm obstacle avoidance path planning method based on an improved artificial potential field method (CN115416016A)":

[0005] The paper, "A Robotic Arm Obstacle Avoidance Path Planning Method Based on an Improved Artificial Potential Field Method," employs the traditional artificial potential field method for obstacle avoidance path planning. It uses a variable step-size search method to escape local minima and a polynomial interpolation method to smooth the trajectory. This paper constructs an objective function in joint space based on the traditional artificial potential field method for obstacle avoidance path planning. When the robot arm's motion gets stuck in a local minimum, a variable step-size search is used to construct an intermediate point to escape the local minimum, eliminating the jitter problem in complex obstacle environments. Furthermore, a fifth-order polynomial interpolation method is combined to optimize the robot arm's trajectory, obtaining the complete trajectory of each joint.

[0006] This invention is "a bidirectional obstacle avoidance path planning method for a handling robot based on an improved artificial potential field method." It uses a lidar as the obstacle detection sensor and proposes adaptive step size, adaptive repulsion coefficient, and adaptive repulsion range. Although the adaptive step size is also variable, it increases with the repulsion force generated by the obstacle, thus accelerating escape from the obstacle's range and reducing the probability of getting trapped in local optima. Furthermore, in complex environments, traditional artificial potential field methods for obstacle avoidance path planning easily get trapped in local minima, even with variable step size search methods. This invention adds a virtual gravity force in Cartesian space, pointing towards the nearest edge point of the nearest obstacle detected by the lidar, quickly escaping local minima and effectively overcoming the problems of unreachable target poses or getting trapped in local optima, leading to stagnation or oscillation near obstacles. Furthermore, the adaptive repulsive force range can smooth the movement trajectory of the transport robot. Therefore, this invention uses cubic NURBS curves to optimize only the splicing points of bidirectional paths. Using polynomial interpolation for all movement trajectories not only wastes computational resources but also affects the planning efficiency of obstacle avoidance paths, which is not appropriate.

[0007] 2. Technical comparison with "A novel bidirectional RRT* method suitable for joint space path planning of robotic arms (CN114310904A)":

[0008] The paper, "A Novel Bidirectional RRT* Method for Path Planning in Robotic Arm Joint Space," employs a bidirectional fast random search tree (RRT) method combined with the traditional artificial potential field method, and utilizes a direct connection strategy to improve search efficiency. This paper represents obstacle information within joint space and applies an extension strategy based on the artificial potential field method to the target-biased bidirectional RRT method, thereby improving the obstacle avoidance capability of the robotic arm. The proposed direct connection strategy determines whether the latest nodes of two search trees can be directly connected, thus enhancing search efficiency.

[0009] This invention is "A Bidirectional Planning Method for Obstacle Avoidance Paths of a Handling Robot Based on an Improved Artificial Potential Field Method." This invention proposes a bidirectional planning method that uses the midpoint joint angle found in reachable poses. This method simultaneously applies the improved artificial potential field method to calculate the paths from the starting pose to the midpoint pose and from the target pose to the midpoint pose in parallel, reducing computation time and improving planning efficiency. However, the extended strategy based on the artificial potential field method applied to the target-biased bidirectional fast random search tree method cannot be parallelized, especially in complex environments where the computational load is high and it is prone to getting trapped in local minima. Furthermore, in complex environments with many obstacles, the direct connection strategy in joint space is difficult to implement. This invention proposes a proximity criterion: if the Euclidean distance between the current joint angle and the target joint angle is less than the proximity criterion, the robot will directly move to the target joint angle in the next update. In addition, the motion paths obtained by the extended strategy based on the artificial potential field method applied to the target-biased bidirectional fast random search tree method are not smooth enough and cannot be directly used for the operation of the handling robot.

[0010] 3. Technical comparison with "A robot path planning method based on an improved artificial potential field method (CN112068560A)":

[0011] The paper, "A Robot Path Planning Method Based on an Improved Artificial Potential Field Method," introduces a distance factor into the traditional artificial potential field method, improves the repulsive force function in stages, and applies an additional force to escape local minima. By introducing a distance factor to improve the repulsive force field function, the repulsive force on the robot is processed in stages based on whether or not obstacles are crossed. When trapped in a local minimum, the robot is treated as a point mass, and an additional force is added to make the robot move along the tangent direction of the region formed by the safe distance from the obstacle. This overcomes the problem of the traditional artificial potential field method becoming unreachable after getting trapped in local minima.

[0012] This invention is "a bidirectional obstacle avoidance path planning method for a handling robot based on an improved artificial potential field method." It uses a lidar as the obstacle detection sensor and improves the repulsion function by introducing an adaptive repulsion coefficient, an adaptive repulsion range, and the shortest model distance from each joint of the handling robot to the virtual filled concave space obstacle. When the obstacle avoidance path planning gets stuck in a local minimum, a virtual attraction force pointing towards the nearest edge point of the nearest obstacle detected by the lidar is added to escape the local minimum. Because the virtual attraction force always points towards the nearest edge point of the nearest obstacle detected by the lidar, the combined action of the repulsion force, attraction force, and virtual attraction force can always guide the handling robot to quickly overcome obstacles. However, when adding additional forces to make the robot move along the tangent direction of the region formed by the safe distance from the obstacle, it is also necessary to calculate the angle between the nearby forces and the direction of movement. Especially when the handling robot is in a complex environment, it needs to consider obstacle avoidance of each joint, making it easier to get stuck in a local minimum formed by one obstacle and then into a smaller local minimum formed by multiple obstacles. Therefore, its practicality needs to be improved. Summary of the Invention

[0013] To address the issue that traditional artificial potential field methods for path planning in transport robots often result in getting stuck in local optima, causing them to stagnate or oscillate near obstacles and thus making the target pose unreachable, this invention proposes a bidirectional obstacle avoidance path planning method for transport robots based on an improved artificial potential field method. This method simultaneously calculates the path from the starting pose to the midpoint pose and the path from the target pose to the midpoint pose. This not only reduces the probability of getting stuck in local optima but also provides a method to escape local optima in complex environments, reducing computation time and improving planning efficiency.

[0014] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0015] This invention provides a bidirectional obstacle avoidance path planning method for a handling robot based on an improved artificial potential field method, comprising the following steps:

[0016] Step 1: Initialize the transport task by setting the starting pose q of the transport robot in Cartesian space. start Target pose q target With an obstacle threshold R, the LiDAR scans the surrounding environment of the handling robot and builds an obstacle model, calculating in real time the shortest model distance from each obstacle to the handling robot and the range ρ of the repulsive force generated by each obstacle. 0,i ;

[0017] Step 2: Solve the inverse kinematics to obtain the starting pose q in Cartesian space. start and target pose q target Converted to the starting joint angle θ in joint space startand target joint angle θ target Determine the target joint angle θ target Is the corresponding pose reachable?

[0018] Step 3, if the target joint angle θ target If the target pose is unreachable, display "Target pose unreachable" and end the algorithm; otherwise, determine the midpoint joint angle θ. mid =(θ start +θ target Is the pose corresponding to ) / 2 reachable?

[0019] Step 4, if the midpoint joint angle θ mid If the corresponding pose is reachable, then set the midpoint joint angle θ. mid Starting point joint angle θ start and target joint angle θ target End joint angle θ goal That is, planning from the starting joint angle θ start Midpoint joint angle θ mid The path s1 and the planning from the target joint angle θ target Midpoint joint angle θ mid The path s2, if the midpoint joint angle θ mid If the corresponding pose is unreachable, then the midpoint joint angle θ is used. mid Starting from the joint angle θ, iteratively calculate... mid The pose corresponding to +Δθ is reachable; record θ at this point. mid +Δθ value and assign it to θ mid ;

[0020] Step 5: Simultaneously and in parallel calculate the joint angle θ from the starting point by applying the improved artificial potential field method. start Midpoint joint angle θ mid The path s1 and the angle θ from the target joint target Midpoint joint angle θ mid Path s2;

[0021] Step 6: Concatenate path s1 and path s2 to obtain the total path s. total ;

[0022] Step 7: Use a cubic NURBS curve to plot the total path s total The joints are smoothed.

[0023] As a further improvement of the present invention, in steps 2 and 3, the target joint angle θ is determined. target and midpoint joint angle θ mid Whether something is reachable depends on the current joint angle θ of the handling robot. currIn the corresponding pose, is the shortest model distance between each joint and all obstacle surfaces greater than or equal to the range of their repulsive force ρ? 0,i If it is achievable, then it is attainable; otherwise, it is unattainable. The range of the repulsive force is... The model area S of the i-th obstacle i Related.

[0024] As a further improvement of the present invention, in step 4, the midpoint joint angle θ under the achievable pose is found by applying the method. mid The bidirectional planning method will start from the planning starting point joint angle θ start To the target joint angle θ target The path is divided into two segments, one starting from the planned starting point joint angle θ. start Midpoint joint angle θ mid The path s1 and the planning from the target joint angle θ target Midpoint joint angle θ mid The path s2, the planned path s1, and the path s2 can be calculated in parallel on the industrial control computer to find the midpoint joint angle θ. mid The specific steps are as follows: first calculate the midpoint joint angle θ in joint space. mid =(θ start +θ target The pose corresponding to ) / 2 is determined to be reachable. If reachable, the midpoint joint angle θ is found. mid Conversely, (θ) start +θ target Starting from 2 / 2, the achievable pose is found by adjusting the angles of each joint by adding Δθ. Δθ satisfies Δθ=[±1±1±1±1±1±1]·t·ε, where t is the number of iterations, ε is the minimum resolution of each joint of the handling robot, and the ± values ​​follow a uniform distribution, meaning that the probability of taking + and - is 0.5, until the joint angle (θ) is found. start +θ target The pose corresponding to ) / 2+Δθ can be reached, at which point the midpoint joint angle θ mid =(θ start +θ target ) / 2+Δθ.

[0025] As a further improvement of the present invention, in step 5, the improved artificial potential field method is applied to simultaneously and in parallel calculate the joint angle θ from the starting point. start Midpoint joint angle θ mid The path s1 and the angle θ from the target joint target Midpoint joint angle θ mid The specific steps for path s2 are:

[0026] Step 51: Initialize the parameters of the improved artificial potential field method, including the gravitational coefficient ξ, the gravitational distance constant σ0, the initial repulsive coefficient η0, the repulsive range constant α, the initial step size κ0, and the approximation standard θ. crit Establish a gravitational field U in the joint space att (θ), establish the repulsive force field U of each obstacle in Cartesian space. rep,i (q);

[0027] Step 52, determine ||θ curr -θ mid Is ||2 greater than the approximation standard θ? crit If yes, proceed to step 53; otherwise, proceed to step 59.

[0028] Step 53: Calculate the current joint angle θ of the handling robot. curr Lower gravitational field U att Multiplying the negative gradient of (θ) by the gravitational coefficient ξ yields the gravitational force F. att (θ), calculate the current pose q of the handling robot. curr The repulsive force field U of each obstacle rep,i Multiplying the negative gradient of (q) by the repulsion coefficient η yields the repulsion force F of each obstacle. rep,i (q), then calculate the repulsive force F of each obstacle in Cartesian space. rep (q);

[0029] Step 54: Calculate the current joint angle θ of the handling robot. curr The Jacobian matrix J(q) of each joint is given by formula F. rep (θ)=J T (q)F rep (q) Combine the repulsive force and F in the Cartesian coordinate system rep (q) Transform into the joint space to obtain repulsive force and F rep (θ);

[0030] Step 55: Calculate the resultant force F acting on each joint of the handling robot. total (θ)=F att (θ)+F rep (θ); then calculate the resultant force F. total (θ) The torque generated on each joint;

[0031] Step 56: Calculate the step length θ of the torque driving the movement of each joint. step ;

[0032] Step 57, move to the next joint angle θ next Update the angles of each joint and determine whether a local optimum has been reached. If so, proceed to step 58; otherwise, return to step 52.

[0033] Step 58, add a virtual gravity F in Cartesian space. vir (q) indicates the nearest edge point of the nearest obstacle detected by the lidar, determined by the Jacobian matrix J. T (q) Transform to joint space to obtain virtual gravity F vir (θ), recalculate the resultant force F total (θ)=F att (θ)+F rep (θ)+F vir (θ) The torque generated on each joint, return to step 55;

[0034] Step 59: Set the step length to the joint angle θ at the midpoint of each joint. mid With the current joint angle θ curr The difference, moving to the next joint angle θ next The algorithm ends, and the path is obtained.

[0035] As a further improvement to the present invention, a gravitational potential field is established in the joint space in step 58, with gravitational force F. att (θ) is calculated by the gravitational field U. att The negative gradient of (θ) is obtained by multiplying it by the gravitational coefficient ξ, and a repulsive potential field is established in Cartesian space, with a repulsive force F. rep (q) is calculated by determining the repulsive field U of all obstacles. rep,i The repulsive force F in Cartesian space is obtained by multiplying the negative gradient of (q) by the sum of the corresponding repulsive force coefficients η. rep (q) Finally, through the Jacobian matrix J T (q) Transition to joint space to obtain repulsive force F rep (θ), at which point the resultant force is F total (θ)=F att (θ)+F rep (θ); When trapped in a local optimum, add a virtual gravity F in Cartesian space. vir (q) indicates the nearest edge point of the nearest obstacle detected by the lidar, determined by the Jacobian matrix J. T (q) Transform to joint space to obtain virtual gravity F vir (θ), at which point the resultant force is F total (θ)=F att (θ)+F rep (θ)+F vir (θ), where for the gravitational field U att (θ) Calculate the negative gradient and multiply by the gravitational coefficient to calculate the gravitational force F in the joint space. att (θ) is:

[0036]

[0037] Where ξ is a constant, representing the gravitational coefficient, used to determine the magnitude of the gravitational force. This represents finding the gradient, σ(θ). curr ,θ mid )=||θ curr -θ mid ||2 represents the current joint angle θ of the handling robot. curr With the midpoint joint angle θ mid The 2-norm of each joint angle, σ0 is a constant, to prevent excessive gravitational force from causing collisions with obstacles when the target pose is far away, θ curr -θ mid It is the current joint angle θ of the handling robot. curr The joint angles below and the midpoint joint angle θ mid The difference in the angles of each joint;

[0038] Repulsive field U rep,i Multiplying the negative gradient of (q) by the repulsive force coefficient η yields the repulsive force F generated by the i-th obstacle in Cartesian space. rep,i (q) is:

[0039]

[0040] Where ρ(q) start ,q obst,i )=||q start -q obst,i ||2 represents the current pose of the transport robot q curr The shortest model distance between the i-th obstacle surface and the i-th obstacle surface. The shortest model distance may not be the actual distance. It represents the distance from each joint of the transport robot to the virtual filled concave space obstacle. ρ 0,i It is an adaptive quantity, representing the range of the repulsive force generated by the i-th obstacle, and is related to the model area S of the i-th obstacle. i Related, its expression Where α is the repulsive force range constant, satisfying 0 < α < 1, and the repulsive force coefficient η is an adaptive quantity, initialized as a constant η0, and updated in subsequent iterative calculations according to the following formula:

[0041]

[0042] Where n is the number of obstacles detected by the lidar, and tanh is the arctangent function;

[0043] When trapped in a local minimum, a virtual gravitational force F is added in Cartesian space pointing towards the nearest edge point of the nearest obstacle detected by the lidar. vir (q), its expression is Where η is the repulsion coefficient, ρ(q) curr ,q point() is the Euclidean distance between the current pose of the transport robot and the nearest obstacle edge.

[0044] The step length θ of each joint of the transport robot step It is an adaptive quantity that satisfies the following formula:

[0045]

[0046] Where, τ i Let ||τ||2 represent the torque acting on the i-th joint, ||τ||2 represent the 2-norm, and κ be the step size coefficient, which is related to the ratio of attraction and repulsion, and is updated according to the following formula:

[0047]

[0048] Where κ0 is the initial step size.

[0049] As a further improvement of the present invention, in step 7, the total path s is analyzed using a cubic NURBS curve. total The specific steps for smoothing the splice joint are as follows: First, take the last 5 odd-numbered points in ascending order from path s1, and then take the last 5 odd-numbered points in reverse order from path s2 to obtain the point set x. point Finally, these point sets x point As the shape points on the cubic NURBS curve, the smooth total path s is obtained by inversely finding its control points. total .

[0050] The beneficial effects of adopting the above technical solution are as follows:

[0051] This invention improves obstacle avoidance path planning efficiency by combining the establishment of a repulsive potential field in Cartesian space and a gravitational potential field in joint space to find the reachable midpoint pose and realize bidirectional path planning of the path from the starting pose to the midpoint pose and the path from the target pose to the midpoint pose in parallel.

[0052] Then, obstacle avoidance paths are planned based on the improved artificial potential field method. The improvements to the artificial potential field method are reflected in the following: the repulsion coefficient η of the obstacle is an adaptive quantity; the closer to the obstacle, the greater the effect of the repulsion coefficient η. When the shortest model distance between each joint of the transport robot and the obstacle in the current pose is close to the obstacle threshold R, the repulsion coefficient of the obstacle approaches infinity, effectively preventing collisions between the transport robot and the obstacle. The range of action of the obstacle repulsion ρ0 is an adaptive quantity, related to the size of the obstacle model area. The larger the obstacle model area, the larger the range of action, reducing the probability of getting trapped in local optima, and the path will be smoother, avoiding large path corners. The step size θ for each movement... step It is an adaptive quantity, related to the step size coefficient κ and the magnitude of the applied torque, with an adaptive step size θ. stepThe length increases with the repulsive force generated by the obstacle, thus accelerating escape from the obstacle's range and reducing the probability of getting trapped in a local optimum. Even if trapped in a local optimum in a complex environment, a virtual gravitational force F is added in Cartesian space, pointing towards the nearest edge point of the nearest obstacle detected by the lidar. vir (q) quickly escapes local optima, effectively overcoming the problem of the target pose being unreachable or getting stuck in local optima, causing the device to stagnate or oscillate near obstacles. Attached Figure Description

[0053] Figure 1 This is a flowchart of a bidirectional obstacle avoidance path planning method for a handling robot based on an improved artificial potential field method, according to the present invention.

[0054] Figure 2 This is a detailed flowchart of the improved artificial potential field method in the bidirectional obstacle avoidance path planning method for a handling robot based on the improved artificial potential field method of the present invention.

[0055] Figure 3 This is a schematic diagram of the shortest model distance in this invention;

[0056] Figure 4 This is the planning result of the implementation case in the MATLAB simulation environment. Detailed Implementation

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

[0058] The embodiments of the present invention will now be described in detail with reference to the accompanying drawings. (See attached drawings.) Figure 1 and attached Figure 2 The described embodiments are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0059] Please see Figure 1 The aforementioned bidirectional obstacle avoidance path planning method for a handling robot based on an improved artificial potential field method includes the following steps:

[0060] Step 1: Initialize the transport task by setting the starting pose q of the transport robot in Cartesian space. start Target pose q target With an obstacle threshold R, the LiDAR scans the surrounding environment of the handling robot and builds an obstacle model, calculating in real time the shortest model distance from each obstacle to the handling robot and the range ρ of the repulsive force generated by each obstacle. 0,i ;

[0061] Step 2: Solve the inverse kinematics to obtain the starting pose q in Cartesian space. start and target pose q targetConverted to the starting joint angle θ in joint space start and target joint angle θ target Determine the target joint angle θ target Is the corresponding pose reachable?

[0062] Step 3, if the target joint angle θ target If the target pose is unreachable, the message "Target pose unreachable" is displayed, ending the bidirectional obstacle avoidance path planning algorithm for the transport robot based on the improved artificial potential field method. Otherwise, the midpoint joint angle θ is determined. mid =(θ start +θ target Is the pose corresponding to ) / 2 reachable?

[0063] Step 4, if the midpoint joint angle θ mid If the corresponding pose is reachable, then set the midpoint joint angle θ. mid Starting point joint angle θ start and target joint angle θ target End joint angle θ goal That is, planning from the starting joint angle θ start Midpoint joint angle θ mid The path s1 and the planning from the target joint angle θ target Midpoint joint angle θ mid The path s2, if the midpoint joint angle θ mid If the corresponding pose is unreachable, then the midpoint joint angle θ is used. mid Starting from the joint angle θ, iteratively calculate... mid The pose corresponding to +Δθ is reachable; record θ at this point. mid +Δθ value and assign it to θ mid ;

[0064] Step 5: Simultaneously and in parallel calculate the joint angle θ from the starting point by applying the improved artificial potential field method. start Midpoint joint angle θ mid The path s1 and the angle θ from the target joint target Midpoint joint angle θ mid Path s2;

[0065] Step 6: Concatenate path s1 and path s2 to obtain the total path s. total ;

[0066] Step 7: Use a cubic NURBS curve to plot the total path s total The joints are smoothed.

[0067] In step 1, the starting pose q start =[x0 y0 z0 u0 v0 w0] TTarget pose q target =[x1 y1 z1u1 v1 w1] T Where [x0 y0 z0] T [x1 y1 z1] T These represent the starting position and target position of the end effector of the handling robot, respectively. [u0 v0 w0] T [u1 v1 w1] T These represent the Euler angles of the end effector of the handling robot at the starting position and the target position, respectively. The LiDAR scans the surrounding environment of the handling robot and builds an obstacle model. Based on the obstacle model, it calculates the closest distances of all joints to the obstacle model surface. Preferably, the closest distances of all joints to the obstacle surface may not be the actual distances, such as... Figure 3 As shown, when the obstacle is concave, it will be regarded as a planar obstacle, that is, the concave space will be virtually filled.

[0068] In step 2, the starting joint angle θ start =[θ 1,0 θ 2,0 θ 3,0 θ 4,0 θ 5,0 θ 6,0 ] T Target joint variable θ target =[θ 1,1 θ 2,1 θ 3,1 θ 4,1 θ 5,1 θ 6,1 ] T Determine the target joint variable θ target Whether it is reachable depends on the angle of each joint of the handling robot being θ. target At that time, the shortest model distance between each joint and all obstacle surfaces is greater than or equal to the range of its repulsive force ρ. 0,i If it is achievable, then it is attainable; otherwise, it is unattainable. The range of the repulsive force is... The model area S of the i-th obstacle i Related;

[0069] In step 3, the midpoint pose θ is determined. mid The criteria for determining whether something is reachable are the same as in step 2;

[0070] In step 4, the calculation is iteratively performed with Δθ as the radius until θ is reached. mid The specific steps to achieve the pose corresponding to +Δθ are: at the midpoint θ miThe reachable pose is found by adjusting the angles of each joint, where Δθ satisfies Δθ=[±1 ±1±1 ±1 ±1 ±1]·t·ε. Here, ε is the minimum resolution of each joint of the handling robot, t is the number of iterations, and the ± values ​​follow a uniform distribution, meaning the probability of taking + and - is both 0.5.

[0071] Please see Figure 2 In step 5, the improved artificial potential field method is applied to simultaneously and in parallel calculate the joint angle θ from the starting point. start Midpoint joint angle θ mid The path s1 and the angle θ from the target joint target Midpoint joint angle θ mid The specific operation of path s2 is:

[0072] Step 51: Initialize the parameters of the improved artificial potential field method, including the gravitational coefficient ξ, the gravitational distance constant σ0, the initial repulsive coefficient η0, the repulsive range constant α, the initial step size κ0, and the approximation standard θ. crit ;

[0073] Step 52, determine ||θ| at the current joint angle. curr -θ mid Is ||2 greater than the approximation standard θ? crit If yes, proceed to step 53; otherwise, proceed to step 59.

[0074] Step 53, establish a gravitational field U in the joint space. att (θ), calculated by the current joint angle θ of the handling robot curr Lower gravitational field U att Multiplying the negative gradient of (θ) by the gravitational coefficient ξ yields the gravitational force F. att (θ), establish the repulsive force field U of each obstacle in Cartesian space. rep,i (q), where 1≤i≤n, and n is the number of obstacles detected by the lidar. Calculate the position q of the transport robot at its current pose. curr The repulsive force field U of the i-th obstacle rep,i Multiplying the negative gradient of (q) by the repulsion coefficient η yields the repulsion force F of the i-th obstacle. rep,i (q);

[0075] Step 54: Calculate the current joint angle θ of the handling robot. curr The Jacobian matrix J(q) of each joint is given by formula F. rep (θ)=J T (q)F rep (q) Combine the repulsive force and F in the Cartesian coordinate system rep (q) Transform into the joint space to obtain repulsive force and F rep (θ);

[0076] Step 55: Calculate the resultant force F acting on each joint of the handling robot. total (θ)=F att (θ)+F rep (θ); then calculate the resultant force F. total (θ) The torque τ generated on each joint;

[0077] Step 56: Calculate the step length θ of each joint movement driven by the torque τ. step ;

[0078] Step 57, the angles of each joint at the next moment are θ next =θ curr +θ step Update the angles of each joint and determine whether a local optimum has been reached. If so, proceed to step 58; otherwise, return to step 52.

[0079] Step 58, add a virtual gravity F in Cartesian space. vir (q), the direction points to the nearest edge point of the nearest obstacle detected by the lidar, and its mathematical expression is: Where η is the repulsion coefficient, It is the Euclidean distance between the current pose of the transport robot and the nearest obstacle edge point, expressed by the Jacobian matrix J. T (q) Transform to joint space to obtain virtual gravity F vir (θ), recalculate the resultant force F total (θ)=F att (θ)+F rep (θ)+F vir (θ) The torque generated on each joint, return to step 55;

[0080] Step 59, at this time, the angles θ of each joint of the transport robot in its current pose. curr With the midpoint joint angle θ mid Extremely close, but with weaker gravity at this point, to prevent jamming or slow iterative calculations, the target pose, θ, will be reached directly in the next step. next =θ mid The step length is set as the joint angle θ at the midpoint of each joint. mid With the current joint angle θ curr The difference, moving to the next joint angle θ next The improved artificial potential field algorithm is then completed, and the path is obtained.

[0081] Step 6: Concatenate path s1 in the forward direction and path s2 in the reverse direction to obtain the total path s. total ;

[0082] Step 7: Use a cubic NURBS curve to plot the total path s totalThe specific steps for smoothing the splice joint are as follows: First, take the last 5 odd-numbered points in ascending order from path s1, and then take the last 5 odd-numbered points in reverse order from path s2 to obtain the point set x. point Finally, these point sets x point As the shape points on the cubic NURBS curve, the smooth total path s is obtained by inversely finding its control points. total ;

[0083] Specifically, the gravitational field U established in the joint space in step 53 att (θ) is:

[0084]

[0085] Where, σ(θ) curr ,θ mid )=||θ curr -θ mid ||2 represents the current joint angle θ of the handling robot. curr With the midpoint joint angle θ mid The 2-norm of each joint angle, σ0 is a constant, to prevent excessive gravity from being generated when the target pose is far away, which could lead to a collision with the obstacle;

[0086] Calculate the current joint angle θ of the handling robot curr Lower gravitational field U att Multiplying the negative gradient of (θ) by the gravitational coefficient ξ yields the gravitational force F. att (θ), its expression is:

[0087]

[0088] Where ξ is the gravitational coefficient, a constant used to determine the magnitude of the gravitational effect, and ▽ represents the gradient;

[0089] Establish a repulsive force field U for each obstacle in Cartesian space. rep,i (q) is:

[0090]

[0091] Where 1≤i≤n, n is the number of obstacles detected by the lidar, ρ(q start ,q obst,i ) represents the current pose of the transport robot. curr The shortest model distance between the i-th obstacle surface and the i-th obstacle surface. The shortest model distance may not be the actual distance. It represents the distance from each joint of the transport robot to the virtual filled concave space obstacle. ρ 0,i It is an adaptive quantity, representing the range of the repulsive force generated by the i-th obstacle, and is related to the model area S of the i-th obstacle. i Related, its expression Where α is the repulsive force range constant, satisfying 0 < α < 1;

[0092] Calculate the current pose q of the handling robot. curr The repulsive force field U of the i-th obstacle rep,i Multiplying the negative gradient of (q) by the repulsion coefficient η yields the repulsion force F of the i-th obstacle. rep,i (q), its expression is:

[0093]

[0094] Where η is the repulsion coefficient, an adaptive quantity that satisfies η >> ξ, and is initialized as a constant η0. It is updated in subsequent iterative calculations according to the following formula:

[0095]

[0096] Where tanh is the arctangent function;

[0097] Calculate the repulsive force F of all obstacles in Cartesian space. rep (q), its expression is:

[0098]

[0099] Where n is the number of obstacles detected by the lidar;

[0100] Specifically, in step 56, the step length θ of the torque τ driving the movement of each joint is calculated. step for:

[0101]

[0102] Where, τ i Let ||τ||² represent the torque acting on the i-th joint, ||τ||² represent the 2-norm, and κ be the step size coefficient, satisfying the following equation:

[0103]

[0104] Where κ0 is a constant representing the initial step size, it can be seen that the greater the repulsive force generated by the obstacle, the larger the step size will be, thus accelerating the escape from the obstacle's effective range. The maximum step size is approximately twice the original step size;

[0105] In step 7, because there may be a sudden change in direction at the junction of path s1 and path s2, a cubic NURBS curve is used for smoothing. The total path s is then smoothed using a cubic NURBS curve. total The specific steps for smoothing the splice joint are as follows: first, take the last 5 odd-numbered points in forward order from path s1, and then take the last 5 odd-numbered points in reverse order from path s2 to obtain the point set x. pointFinally, these point sets x point As the shape points on the cubic NURBS curve, the smooth total path s is obtained by inversely finding its control points. total .

[0106] The planning results of this invention's implementation examples in the MATLAB simulation environment are as follows: Figure 4 As shown in the figure, the center is a model of the transport robot, and the surrounding square objects are obstacles. The curve represents the actual running trajectory of the end effector of the transport robot obtained by the bidirectional path planning method for obstacle avoidance of the transport robot based on the improved artificial potential field method. The results show that the transport robot can bypass obstacles and reach the target pose in complex environments under the method of the present invention, and the running trajectory is smooth.

[0107] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A bidirectional obstacle avoidance path planning method for a transport robot based on an improved artificial potential field method, characterized in that, Includes the following steps: Step 1: Initialize the transport task by setting the starting pose of the transport robot in Cartesian space. Target pose and obstacle threshold The lidar scans the surrounding environment of the handling robot and builds an obstacle model, calculating in real time the shortest model distance from each obstacle to the handling robot and the range of the repulsive force generated by each obstacle. ; Step 2: Solve the inverse kinematics to obtain the starting pose in Cartesian space. and target pose Converted to starting joint angle in joint space and target joint angle Determine the angle of the target joint Is the corresponding pose reachable? Step 3, if the target joint angle If the target pose is unreachable, the algorithm will display "Target pose unreachable" and terminate the current algorithm. Otherwise, the midpoint joint angle will be checked. Is the corresponding pose reachable? Step 4, if the midpoint joint angle If the corresponding pose is reachable, then set the midpoint joint angle. Starting point joint angle and target joint angle End joint angle That is, planning from the perspective of the starting point joint. Midpoint joint angle path Planning from the perspective of target joints Midpoint joint angle path If the midpoint joint angle If the corresponding pose is unreachable, then the midpoint joint angle is used. Starting from the joint angle, iteratively calculate... The corresponding pose is reachable; record this position. value and assign to ; Step 5: Simultaneously and in parallel calculate the joint angle from the starting point by applying the improved artificial potential field method. Midpoint joint angle path and from the angle of the target joint Midpoint joint angle path ; Step 6, change the path With path By splicing the parts together, we obtain the total path. ; Step 7: Use cubic NURBS curves to plot the total path. The joints are smoothed.

2. The bidirectional obstacle avoidance path planning method for a transport robot based on the improved artificial potential field method as described in claim 1, characterized in that: In steps 2 and 3, the target joint angle is determined. and midpoint joint angle Whether something is reachable depends on the current joint angle of the handling robot. In the corresponding pose, is the shortest model distance between each joint and all obstacle surfaces greater than or equal to the range of their repulsive force? If it is achievable, then it is attainable; otherwise, it is unattainable. The range of the repulsive force is... With the The size of the model of each obstacle Related, R It is the obstacle threshold. It is the repulsive force range constant, satisfying .

3. The bidirectional obstacle avoidance path planning method for a transport robot based on the improved artificial potential field method as described in claim 1, characterized in that: In step 4, the midpoint joint angle is found in the reachable pose. The two-way planning method will start from the planning starting point joint perspective. to the target joint angle The path is divided into two segments, one starting from the planned starting point joint. Midpoint joint angle path Planning from the perspective of target joints Midpoint joint angle path , planning path and path Parallel computation can be performed simultaneously on the industrial computer to find the midpoint joint angle. The specific steps are as follows: first calculate the midpoint joint angle in joint space. Given the corresponding pose, determine if it is reachable; if so, find the midpoint joint angle. Conversely, Starting from, by adding Adjust the angles of each joint to find the corresponding reachable pose. satisfy ,in It is the number of iterations. It is the minimum resolution of each joint of the transport robot. The values ​​follow a uniform distribution, that is, take and The probability is 0.5, until the joint angle. The corresponding pose can be reached, at which point the midpoint joint angle is... .

4. The bidirectional obstacle avoidance path planning method for a transport robot based on the improved artificial potential field method as described in claim 1, characterized in that: In step 5, the improved artificial potential field method is applied to simultaneously and in parallel calculate the angle from the starting joint. Midpoint joint angle path and from the angle of the target joint Midpoint joint angle path The specific steps are: Step 51: Initialize the parameters of the improved artificial potential field method, including the gravitational coefficient. Gravitational distance constant Initial repulsion coefficient Repulsive force range constant Initial step size and approaching standard Establish a gravitational field in the joint space Establish repulsive fields for each obstacle in Cartesian space. ; Step 52, Determine Is it greater than the approximation standard? If yes, proceed to step 53; otherwise, proceed to step 59. Step 53: Calculate the current joint angles of the handling robot. Lower gravitational field negative gradient multiplied by gravity coefficient Gain gravity Calculate the current pose of the handling robot Repulsive fields of various obstacles The negative gradient multiplied by the repulsion coefficient Obtain the repulsive force of each obstacle Then calculate the repulsive force of each obstacle in Cartesian space. ; Step 54: Calculate the current joint angles of the handling robot. Jacobian matrix of each joint According to the formula The repulsive force in the Cartesian coordinate system and Transformed into joint space, repulsive force and ; Step 55: Calculate the resultant force on each joint of the handling robot. Then calculate the resultant force. The torques generated on each joint; Step 56: Calculate the step length of each joint movement driven by the torque. ; Step 57, move to the next joint angle Update the angles of each joint and determine whether a local optimum has been reached. If so, proceed to step 58; otherwise, return to step 52. Step 58, add a virtual gravity to Cartesian space. The direction is pointed towards the nearest edge point of the nearest obstacle detected by the lidar, through the Jacobian matrix. Transform into joint space to obtain virtual gravity. Recalculate the resultant force The torques acting on each joint are returned to step 55; Step 59: Set the step length as the midpoint joint angle of each joint. With the current joint angle The difference, moving to the next joint angle The algorithm ends, and the path is obtained.

5. The bidirectional obstacle avoidance path planning method for a transport robot based on the improved artificial potential field method as described in claim 4, characterized in that: In step 58, a gravitational potential field is established in the joint space. By calculating the gravitational field negative gradient multiplied by gravity coefficient The result is a repulsive potential field established in Cartesian space, with repulsive force... It is done by calculating the repulsive field of all obstacles. The negative gradient is multiplied by the corresponding repulsion coefficient. The repulsive force in Cartesian space. Ultimately, through the Jacobian matrix Transitioning to joint space, a repulsive force is obtained. At this point, the combined force is When trapped in a local optimum, add virtual gravity in Cartesian space. The direction is pointed towards the nearest edge point of the nearest obstacle detected by the lidar, through the Jacobian matrix. Transform into joint space to obtain virtual gravity. At this point, the combined force is Among them, the gravitational field Calculate the gravity in joint space by finding the negative gradient and multiplying it by the gravity coefficient. for: ； in, It is a constant, representing the gravitational coefficient, used to determine the magnitude of the gravitational force. This indicates calculating the gradient. Indicates the current joint angle of the handling robot Angle with midpoint joint The 2-norm of each joint angle, It is a constant to prevent excessive gravitational force from causing a collision with an obstacle when the target pose is far away. It is the current joint angle of the handling robot. The joint angles of each joint and the midpoint joint angle The difference in the angles of each joint; Repulsive field The negative gradient multiplied by the repulsion coefficient Get the first The repulsive force generated by an obstacle in Cartesian space for: ； in, This indicates the current pose of the transport robot. The following and the first The shortest model distance to the surface of each obstacle. This shortest model distance may not be the actual distance; it represents the distance from each joint of the transport robot to the virtual, filled concave space obstacle. It is an adaptive quantity, representing the first... The range of the repulsive force generated by the first obstacle is related to the range of the second obstacle. The size of the model of each obstacle Related, its expression ， in R It is the obstacle threshold, where It is the repulsive force range constant, satisfying repulsion coefficient It is an adaptive quantity, initialized as a constant. In subsequent iterative calculations, the result is updated according to the following formula: ； in, is the number of obstacles detected by the lidar, and tanh is the arctangent function; When trapped in a local minimum, a virtual gravity is added to the Cartesian space pointing towards the nearest edge point of the nearest obstacle detected by the lidar. Its expression is ,in, It is the repulsion coefficient. It is the Euclidean distance between the current pose of the transport robot and the edge of the nearest obstacle; Step length of each joint of the transport robot It is an adaptive quantity that satisfies the following formula: ； in, Indicates the action in the The torque of each joint, Describing the 2-norm, It is the step size coefficient, which is related to the ratio of attractive and repulsive forces, and is updated according to the following formula: ； in, It is the initial step size.

6. The bidirectional obstacle avoidance path planning method for a transport robot based on the improved artificial potential field method as described in claim 5, characterized in that: In step 7, the total path is calculated using a cubic NURBS curve. The specific steps for smoothing the joints are as follows: First, start from the path... Take the last 5 odd-numbered points in ascending order, then proceed from the path. Take the last 5 odd-numbered points in reverse order to obtain the point set. Finally, these point sets As the shape points on the cubic NURBS curve, the smooth total path is obtained by inversely finding its control points. .

Images