Robot path adjustment method and device based on DWA algorithm

Through the improved azimuth evaluation function and obstacle distance evaluation function, combined with pheromone update and dynamic adjustment factors, the DWA algorithm is optimized, which solves the problems of direction deviation and complex environmental adaptability in robot path planning, and improves motion accuracy and efficiency .

CN119047672APending Publication Date: 2024-11-29LIAO NING GONG CHENG JI SHU DA XUE E ER DUO SI YAN JIU YUAN
View PDF 0 Cites 2 Cited by

Patent Information

Application Number
CN202411130030.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-16
Publication Date
2024-11-29

AI Technical Summary

Technical Problem

During the motion process of the existing robot path planning method based on DWA algorithm, the azimuth evaluation function design is not perfect enough, resulting in a large deviation from the direction of the robot from the target point, affecting the motion efficiency and accuracy.

Method used

The improved azimuth evaluation function is adopted to consider the distance between the end of the local path trajectory to the end point, the distance between the starting point of the path to the end point, and the angle between the line between the tangent direction and the end point, combined with the obstacle distance evaluation function, through multiple iterations and Pheromone updates optimized paths, dynamically adjusting heuristic functions and volatile factors to improve the accuracy and efficiency of path planning.

Benefits of technology

It improves the accuracy and motion efficiency of the robot to reach the target point, enhances the adaptability to complex environments and the robustness of path planning, and optimizes the overall path quality and safety.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119047672A_ABST
    Figure CN119047672A_ABST
Patent Text Reader

Abstract

Embodiments of the invention disclose a robot path adjustment method and apparatus based on a DWA algorithm. The method comprises the steps of obtaining a pre-generated optimal path of a robot; based on the turning point of the optimal path, setting the turning point as the tail end of each local path track; parameter information of the robot is set based on the optimal path, wherein the parameter information comprises an angle and a speed value; the optimal path is adjusted according to a preset trajectory evaluation model and obstacle information, an adjusted path is obtained, and the trajectory evaluation model evaluates the path trajectory of the robot from each node to the next node based on a DWA algorithm. The DWA algorithm comprises an azimuth angle evaluation function, a speed evaluation function and an obstacle distance evaluation function, and the azimuth angle evaluation function is based on a first distance from the tail end of each local path track to the end point and a second distance from the starting point of each path to the end point; and determining an included angle of a connecting line between the tangential direction of the tail end of each local path track and the end point.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present specification relates to the field of computer technology, and in particular to a robot path adjustment method and device based on a DWA algorithm. Background Art

[0002] With the continuous development of robot technology, robots are used more and more widely in various fields. In the practical application of robots, path planning is a key issue, which directly affects the working efficiency and accuracy of robots.

[0003] At present, a variety of robot path planning methods have been proposed. Among them, the DWA (Dynamic Window Approach) algorithm is a commonly used path planning algorithm. However, the existing robot path planning methods based on the DWA algorithm still have some shortcomings in practical applications.

[0004] In the traditional DWA algorithm, the design of the azimuth evaluation function is often not perfect, which may cause a large deviation between the robot's orientation and the target point direction during the movement, thus affecting the robot's movement efficiency and accuracy. Summary of the invention

[0005] One or more embodiments of the present specification provide a robot path adjustment method and device based on a DWA algorithm, which are used to solve the technical problems raised by the background technology.

[0006] One or more embodiments of this specification adopt the following technical solutions:

[0007] One or more embodiments of this specification provide a robot path adjustment method based on the DWA algorithm, the method comprising:

[0008] The optimal path of the robot generated in advance is obtained; the turning point based on the optimal path is set as the end of each local path trajectory; the parameter information of the robot is set based on the optimal path, and the parameter information includes angle and speed values; the optimal path is adjusted according to a preset trajectory evaluation model and obstacle information to obtain an adjusted path, the trajectory evaluation model is based on the DWA algorithm to evaluate the path trajectory of the robot from each node to the next node, the DWA algorithm includes an azimuth evaluation function, a speed evaluation function and an obstacle distance evaluation function, and the azimuth evaluation function is determined based on the first distance between the end of each local path trajectory and the end point, the second distance between the starting point and the end point of each path, and the angle between the tangent direction of the end of each local path trajectory and the line connecting the end point.

[0009] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0010] Improved direction accuracy: The improved azimuth evaluation function takes into account the first distance between the end of each local path trajectory and the end point, the second distance between the start point and the end point of each path, and the angle between the tangent direction of the end of each local path trajectory and the line connecting the end point. This enables the robot to adjust its direction more accurately during movement, reduce the deviation from the target point direction, and improve the accuracy of the robot reaching the target point.

[0011] Improved movement efficiency: Since the robot can point to the target point more accurately, unnecessary movements and adjustments caused by directional deviations are reduced, thereby improving the robot's movement efficiency and saving time and energy.

[0012] Enhance the rationality of path planning: Through a more complete azimuth evaluation function, the trajectory evaluation model can more comprehensively evaluate the robot's path trajectory. This helps to generate a more reasonable adjustment path, allowing the robot to avoid obstacles more effectively and approach the target point more quickly.

[0013] Improve adaptability to complex environments: Accurate azimuth control enables the robot to better adjust its path when facing complex environments and variable obstacle distribution, improving the robot's ability to adapt to various complex situations in practical applications.

[0014] Optimizing the overall path quality: The improved azimuth evaluation function helps to optimize the robot's path planning, making the generated path smoother and more direct, reducing the tortuosity and redundancy of the path, thereby improving the overall path quality.

[0015] Furthermore, before obtaining the pre-generated optimal path of the robot, the method further includes:

[0016] When planning the path of the robot in the current iteration, the current pheromone concentration and the preset heuristic function on the path updated in the previous iteration are obtained, and the preset heuristic function is determined based on the dynamic adjustment factor, the current node and the next node. The dynamic adjustment factor is used to gradually weaken the influence of the preset heuristic function on the robot path planning as the number of iterations increases;

[0017] Determine the pheromone concentration change amount corresponding to the robot moving from each node to the next node according to the current pheromone concentration;

[0018] Determine the heuristic parameters corresponding to the robot's transition from each node to the next node according to the preset heuristic function;

[0019] Determine the first transition probability of the robot from each node to the next node in the current iteration according to the change in pheromone concentration corresponding to each node to the next node and the heuristic parameter corresponding to each node to the next node;

[0020] After generating the planned path of the robot in the current iteration based on the first transition probability, updating the current pheromone concentration through the pheromone update function to obtain the latest pheromone concentration on the path;

[0021] Based on the latest pheromone concentration and the preset heuristic function, the second transition probability of the robot from each node to the next node in the next iteration is determined, the planned path of the robot in the next iteration is generated based on the second transition probability, and the optimal path of the robot is obtained after the last iteration.

[0022] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0023] Improve path planning efficiency: By presetting the heuristic function and pheromone concentration to determine the robot's transfer probability from each node to the next node, the robot can be guided to find a better path more quickly, reducing unnecessary searches and calculations, thereby improving the efficiency of path planning.

[0024] Adaptive adjustment: The introduction of dynamic adjustment factors makes the influence of the preset heuristic function on the robot's path planning gradually weaken as the number of iterations increases. This means that in the initial stage, the heuristic function can help the robot explore potential paths faster, and as the iterations proceed, the effect of pheromone concentration gradually increases, making path planning more dependent on actual search experience and improving the adaptive ability of path planning.

[0025] Optimize path quality: Determine the transfer probability based on pheromone concentration and heuristic parameters, and comprehensively consider the historical experience of the path and the current heuristic information to generate a better planning path. By continuously iterating and updating the pheromone concentration, the path quality is continuously optimized, and the optimal path is finally obtained.

[0026] Enhanced robustness: Through multiple iterations and pheromone updates, this method can find a suitable path under different environmental and task conditions. Even in the face of complex environments and uncertain factors, it can adapt to changes by continuously adjusting the transition probability, which enhances the robustness of the robot's path planning.

[0027] Improve solution accuracy: Through continuous iteration and optimization of transfer probability, the robot can find the optimal path more accurately, reduce path errors, and improve the solution accuracy of path planning.

[0028] Further, the obstacle distance evaluation function includes a minimum distance function between the end point and a stationary obstacle and a minimum distance function between the end point and a dynamic obstacle;

[0029] The DWA algorithm expression is:

[0030] G(v,w)=αNhead(v,w)+βvel(v,w)+γdist s (v,w)+δdist d (v,w);

[0031] Among them, Nhead(v,w) is the azimuth evaluation function, vel(v,w) is the velocity evaluation function, and dist s (v,w) is the minimum distance function between the end point and the stationary obstacle, dist d (v,w) is the minimum distance function between the end point and the dynamic obstacle, α, β, γ and δ are the preset weight values ​​of the azimuth evaluation function, the speed evaluation function, the minimum distance function between the end point and the static obstacle and the minimum distance function between the end point and the dynamic obstacle, respectively.

[0032] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0033] Improve safety: The obstacle distance evaluation function includes the minimum distance function between the end point and the static obstacle and the minimum distance function between the end point and the dynamic obstacle. By incorporating these two functions into the DWA algorithm, the distance between the robot and the obstacle during movement can be better considered, thereby reducing the risk of collision and improving the safety of robot operation.

[0034] Enhanced adaptability: The algorithm takes dynamic obstacles into account, allowing the robot to better cope with changes in the environment when planning its path. Whether facing static or dynamic obstacles, the robot can make corresponding adjustments based on the distance evaluation function, enhancing the robot's adaptability to complex environments.

[0035] Optimize path planning: By comprehensively considering the azimuth evaluation function, speed evaluation function, and obstacle distance evaluation function, and setting preset weight values ​​for each function, the robot's path selection can be more comprehensively evaluated. This can generate a more reasonable path, avoid unnecessary detours or risky behaviors, and improve the quality and efficiency of path planning.

[0036] Improved flexibility: The setting of preset weight values ​​allows the importance of each function in the algorithm to be adjusted according to actual needs and scenarios. For example, in an environment with dense obstacles, the weight of the obstacle distance evaluation function can be increased to ensure the safety of the robot; in the case of high speed requirements, the weight of the speed evaluation function can be appropriately increased. This flexibility allows the robot to better adapt to different tasks and environmental requirements.

[0037] Furthermore, the azimuth evaluation function expression is:

[0038] head(v,ω)=π-θ;

[0039] Among them, Dist NE is the first distance, Dist SE is the second distance, head(v,ω) is the azimuth function, and θ is the included angle.

[0040] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0041] Improve movement accuracy: In the early stage of robot movement, the weight of the azimuth evaluation function is relatively large, which can enable the robot to move toward the target point better, reduce the deviation between the robot's direction and the target point, and improve the accuracy of movement.

[0042] Optimize speed control: In the later stages of the movement, as the target point is approached, the azimuth function decreases and the direction angle evaluation is weakened, allowing the robot to slow down and avoid safety issues such as collisions that may be caused by high-speed movement, while also reaching the target position more accurately.

[0043] Enhanced safety: By reducing the impact of the azimuth evaluation function in the later stages of the movement, safety issues that may be caused by changes in the robot's posture are prevented, thereby improving the robot's safety throughout the entire movement process.

[0044] Adaptive adjustment: This function can make adaptive adjustments based on the distance between the robot and the target point. In the early stage of movement, it focuses on direction adjustment and rapid approach to the target, while in the later stage, it focuses on deceleration and precise position control, so that the robot's movement is more in line with actual needs.

[0045] Improve the quality of path planning: The reasonable design of the azimuth evaluation function helps to select a more appropriate speed combination as the next motion instruction of the robot, thereby optimizing the robot's motion trajectory and improving the quality of path planning.

[0046] Improve overall performance: Overall, the azimuth evaluation function can play different roles in different stages of the robot's motion, thereby improving the robot's overall motion performance, including accuracy, safety, and efficiency.

[0047] Furthermore, the preset heuristic function is determined based on the dynamic adjustment factor, the initial node, the current node, the next node and the end point, and the expression of the preset heuristic function is: The expression of the dynamic adjustment factor is

[0048] Among them, η ij is the result of the preset heuristic function; λ is a constant, and 0<λ<1; d sj is the distance from the starting node to the next node, d ijis the distance from the current node to the next node, d jE is the distance from the next node to the end point, is the dynamic adjustment factor, N is the current number of iterations, N max is the maximum number of iterations, ε is a preset positive number, and ε is used to maintain Not 0.

[0049] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0050] Global perspective consideration: Through the improved heuristic function, the connection between the end point and the next node is increased, so that the starting node and the end point can be considered from a global perspective during the path search process, avoiding only focusing on the current node and the next node and choosing a path that deviates from the end point, thereby improving the globality and accuracy of path planning.

[0051] Enhance the connection between the starting point and the next node: Select the distance between the starting node and the next node to better increase the connection between the starting node and the next node. It can indicate the relationship between the node to be selected next and the starting node and the end point, which helps to find a better path.

[0052] Approaching the goal more accurately: The introduction of the distance between the next node and the end point can better estimate whether the next node is closer to the end point, which helps to achieve the goal of selecting the shortest path from the starting node to the end point.

[0053] Enhanced global search capability: The dynamic adjustment factor is close to 1 at the beginning of the algorithm and gradually decreases as the number of iterations increases. In the early stage of the algorithm, the strong influence of the heuristic function helps to quickly explore the path; as the iterations proceed, the dynamic adjustment factor gradually decreases, which can enhance the global search capability, avoid falling into the local optimum, and increase the possibility of finding the optimal path.

[0054] Ensure algorithm stability: To prevent the dynamic adjustment factor from being equal to 0 in the later stage of iteration, causing the heuristic function to become 0, a very small positive number is added to ensure the effectiveness of the heuristic function and improve the stability and reliability of the algorithm.

[0055] Furthermore, the updating of the current pheromone concentration by the pheromone update function to obtain the latest pheromone concentration on the path includes:

[0056] According to the pheromone update function τ ij (t+1)=(1-ρ(N c +1))τ ij (t)+ρ(N c +1)*Δτ ij (t) updating the current pheromone concentration to obtain the latest pheromone concentration on the path, Among them, τij (t+1) is the latest pheromone concentration on the path from the current node i to the next node j, τ ij (t) is the current pheromone concentration on the path from the current node i to the next node j, ρ(N c +1) is the current pheromone volatilization factor, ρ(N c ) is the last pheromone volatilization factor, Δτ ij (t) is the increase in the amount of pheromone released on the path from the current node i to the next node j, N cmax is the maximum number of iterations, N c is the current iteration number.

[0057] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0058] Promote path exploration: In the early stage of the algorithm, setting a larger pheromone volatilization factor can make the pheromones on the path previously walked by ants evaporate faster, reduce interference with subsequent ants, and allow ants to better explore new possible paths, increasing the chance of finding the optimal path.

[0059] Dynamic optimization search: According to the algorithm's search status and performance, the value of the volatility factor is dynamically adjusted through the formula, which enables the algorithm to adopt different strategies at different search stages. In the early stage, extensive exploration is encouraged, and in the later stage, more emphasis is placed on strengthening existing good paths, so as to achieve better search results.

[0060] Guarantee algorithm performance: Setting the minimum value of the volatility factor to a small positive number (such as 0.1) can ensure that the algorithm always has a certain degree of randomness and global search capabilities during operation. This avoids the situation where the algorithm may rely entirely on previous path information when the volatility factor is 0, resulting in falling into the local optimum and being unable to find the global optimal path, thereby improving the reliability and stability of the algorithm.

[0061] Improve search efficiency: By reasonably adjusting the pheromone volatilization factor, the algorithm can explore the search space more effectively, reduce unnecessary repeated searches and the selection of wrong paths, thereby improving search efficiency and finding the optimal or near-optimal path more quickly.

[0062] Furthermore, the formula for increasing the amount of pheromone released on the path from the current node i to the next node j is: Among them, D worst is the longest path from the current node i to the next node j, D best is the shortest path from the current node i to the next node j, D k is the current path from the current node i to the next node j.

[0063] Furthermore, the pheromone update function τ ij (t+1)=(1-ρ(N c +1))τ ij (t)+ρ(N c +1)*Δτ ij (t) updating the current pheromone concentration, comprising:

[0064] like Then D best <D k <D worst , indicating that the current path is not the longest or shortest path, and the iterative search continues;

[0065] like Then D k >D worst or D k <D best ;

[0066] If D k >D worst , indicating that the current path is longer than the longest path before, then the current path replaces the previous longest path to become the longest path in the next iteration, and increases the volatility factor of the path to reduce the pheromone on the path; if D k <D best , indicating that the current path is shorter than the previous shortest path, then the current path replaces the previous shortest path to become the shortest path in the next iteration, and reduces the volatility factor of the path, so that the pheromone on the path increases.

[0067] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0068] Continuous optimization: By continuously determining whether the current path is the longest or shortest path, and continuing to iterate when it is not, it helps to find a better path solution.

[0069] Update the longest path: When the current path is found to be longer than the previous longest path, it is replaced with the longest path in the next iteration, and the volatility factor of the path is increased to reduce the pheromone. This can prevent subsequent searches from being overly focused on this longer path that may not be optimal, and encourage the algorithm to continue to explore better paths.

[0070] Update the shortest path: When the current path is found to be shorter than the previous shortest path, it is replaced as the shortest path in the next iteration, and the volatility factor of the path is reduced to increase the pheromone. This helps to strengthen this better shorter path and increase the probability of the algorithm selecting this path in subsequent searches, thereby finding a shorter path faster.

[0071] Balanced search: By dynamically updating the longest and shortest paths and adjusting the corresponding volatility factors, balanced control of the search process is achieved. This not only avoids the algorithm from converging to the local optimal longer path too early, but also strengthens the exploration of better shorter paths, increasing the possibility of finding the global optimal path.

[0072] Improve efficiency: This mechanism of dynamically adjusting pheromones and volatility factors according to path length enables the algorithm to search more targetedly, reduce unnecessary calculation and search time, and improve the overall efficiency of the algorithm.

[0073] Furthermore, the updating of the current pheromone concentration by the pheromone update function to obtain the latest pheromone concentration on the path includes:

[0074] Obtaining a preset pheromone concentration threshold, where the pheromone concentration threshold is used to avoid a local optimal solution in path planning;

[0075] Update the current pheromone concentration by using a pheromone update function to obtain a pheromone concentration to be updated;

[0076] The pheromone concentration to be updated is processed based on the pheromone concentration threshold to obtain the latest pheromone concentration on the path.

[0077] Further, the step of processing the pheromone concentration to be updated based on the pheromone concentration threshold to obtain the latest pheromone concentration on the path includes:

[0078] According to the formula Process the pheromone concentration to be updated to obtain the latest pheromone concentration on the path, where the pheromone concentration threshold is [τ min ,τ max ].

[0079] One or more embodiments of this specification provide a robot path adjustment device based on a DWA algorithm, including:

[0080] A path acquisition unit, which acquires the optimal path of the robot generated in advance;

[0081] A target point setting unit, which sets the turning point of the optimal path as the end of each local path trajectory;

[0082] A parameter setting unit, which sets parameter information of the robot based on the optimal path, wherein the parameter information includes angle and speed values;

[0083] The path adjustment unit adjusts the optimal path according to a preset trajectory evaluation model and obstacle information to obtain an adjusted path. The trajectory evaluation model evaluates the path trajectory of the robot from each node to the next node based on the DWA algorithm. The DWA algorithm includes an azimuth evaluation function, a speed evaluation function and an obstacle distance evaluation function. The azimuth evaluation function is determined based on a first distance between the end of each local path trajectory and the end point, a second distance between the starting point and the end point of each path, and an angle between the tangent direction of the end of each local path trajectory and the line connecting the end point.

[0084] At least one of the above technical solutions adopted in the embodiments of this specification can achieve the following beneficial effects:

[0085] Improved direction accuracy: The improved azimuth evaluation function takes into account the first distance between the end of each local path trajectory and the end point, the second distance between the start point and the end point of each path, and the angle between the tangent direction of the end of each local path trajectory and the line connecting the end point. This enables the robot to adjust its direction more accurately during movement, reduce the deviation from the target point direction, and improve the accuracy of the robot reaching the target point.

[0086] Improved movement efficiency: Since the robot can point to the target point more accurately, unnecessary movements and adjustments caused by directional deviations are reduced, thereby improving the robot's movement efficiency and saving time and energy.

[0087] Enhance the rationality of path planning: Through a more complete azimuth evaluation function, the trajectory evaluation model can more comprehensively evaluate the robot's path trajectory. This helps to generate a more reasonable adjustment path, allowing the robot to avoid obstacles more effectively and approach the target point more quickly.

[0088] Improve adaptability to complex environments: Accurate azimuth control enables the robot to better adjust its path when facing complex environments and variable obstacle distribution, improving the robot's ability to adapt to various complex situations in practical applications.

[0089] Optimizing the overall path quality: The improved azimuth evaluation function helps to optimize the robot's path planning, making the generated path smoother and more direct, reducing the tortuosity and redundancy of the path, thereby improving the overall path quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] In order to more clearly illustrate the technical solutions in the embodiments of this specification or the prior art, the following briefly introduces the drawings required for use in the embodiments or the prior art description. Obviously, the drawings described below are only some embodiments recorded in this specification. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative labor. In the drawings:

[0091] Figure 1 A schematic flow chart of a robot path adjustment method based on a DWA algorithm provided in one or more embodiments of this specification;

[0092] Figure 2 A flowchart of a robot path planning method based on an ant algorithm provided for one or more embodiments of this specification;

[0093] Figure 3 A schematic diagram of the structure of a robot path adjustment device based on a DWA algorithm provided in one or more embodiments of this specification. DETAILED DESCRIPTION

[0094] The embodiments of this specification provide a robot path adjustment method and device based on the DWA algorithm.

[0095] In order to enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions in the embodiments of this specification will be clearly and completely described below in conjunction with the drawings in the embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this specification, not all of the embodiments. Based on the embodiments of this specification, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of this specification.

[0096] Figure 1 A schematic diagram of a process flow of a robot path adjustment method based on a DWA algorithm provided in one or more embodiments of this specification, which process can be executed by a robot path adjustment system. Certain input parameters or intermediate results in the process allow manual intervention and adjustment to help improve accuracy.

[0097] The method steps of the embodiment of this specification are as follows:

[0098] S101, obtaining a pre-generated optimal path of the robot.

[0099] In the embodiments of this specification, the optimal path can be obtained through multiple iterations of the ant colony algorithm, and the optimal path of the robot can be generated through the following specific implementation scheme:

[0100] Data preparation: Determine the parameters required for the ant colony algorithm, such as the number of ants, pheromone volatilization factor, pheromone enhancement factor, etc. Prepare a map of the environment in which the robot will operate, including information such as the starting point, end point, and possible obstacles.

[0101] Ant colony algorithm iteration: Initialize the pheromone matrix and initialize the pheromone on all paths to a small constant. Place a certain number of ants at the starting point. Each ant selects the next node based on the pheromone concentration and heuristic information (such as distance, feasibility, etc.) on the current path until it reaches the end. After the ant completes a path exploration, it updates the pheromone concentration on the path according to the path length. The shorter the path, the more pheromone increases; at the same time, the original pheromone will evaporate according to the volatility factor. Repeat the above steps for multiple iterations to make the ant colony algorithm converge to a better path.

[0102] Determine the optimal path: After multiple iterations, the shortest path is selected from the many paths obtained by the ant colony algorithm as the optimal path.

[0103] Output optimal path: Output the obtained optimal path in a suitable data format so that the robot can use the path to move.

[0104] It should be noted that the ant colony algorithm is an algorithm designed to simulate the behavior of ants in the process of finding food by releasing pheromones to guide their companions. Through multiple iterations, the ant colony algorithm can find a relatively optimal path, but this path may still have some areas that can be improved. The following steps are the steps to adjust the optimal path.

[0105] S102: Setting turning points based on the optimal path as the end points of each local path trajectory.

[0106] In the embodiments of this specification, the above-mentioned setting of the end of each local path trajectory can be implemented by the following specific implementation scheme:

[0107] Determine the turning point of the optimal path: Analyze the optimal path and find the turning point in the path. The turning point can be determined by calculating the change in the direction of the path. For example, when the change in the direction of the path exceeds a certain threshold, the point is considered a turning point.

[0108] Set turning points as the ends of each local path track: According to the determined turning points, mark them as the ends of the local path tracks in sequence.

[0109] S103, setting parameter information of the robot based on the optimal path, the parameter information including angle and speed values.

[0110] In the embodiments of this specification, the parameter information of the robot can be set as follows:

[0111] Set angle: At each node of the optimal path, determine the angle of the robot according to the direction of the path. The starting angle can be determined by calculating the angle between the line connecting each node and the next path point and the reference direction (such as the north direction). For example, the angle can be calculated using the vector method, the coordinates of each node and the next path point are converted into vectors, and then the angle between the two vectors is calculated as the starting angle.

[0112] Set the speed value: Set the appropriate speed value according to actual needs and the performance of the robot. Consider factors such as the urgency of the task, the complexity of the path, the robot's power performance and battery life. You can set multiple speed gears and switch them according to different scenarios. For example, you can set a higher speed in an open area and a lower speed in a narrow or complex area.

[0113] Implement parameter setting: Input the angle and speed values ​​of each node into the robot's control system. The control system controls the robot's movement based on these parameters, making it move according to the set optimal path, starting angle and speed value.

[0114] S104, adjusting the optimal path according to a preset trajectory evaluation model and obstacle information to obtain an adjusted path, wherein the trajectory evaluation model evaluates the path trajectory of the robot from each node to the next node based on a DWA algorithm, wherein the DWA algorithm includes an azimuth evaluation function, a speed evaluation function, and an obstacle distance evaluation function, wherein the azimuth evaluation function is determined based on a first distance between the end of each local path trajectory and the end point, a second distance between the starting point and the end point of each path, and an angle between a tangent direction at the end of each local path trajectory and a line connecting the end point.

[0115] In the embodiments of this specification, the above-mentioned adjustment of the optimal path to obtain the adjusted path can be implemented by the following specific implementation scheme:

[0116] The trajectory evaluation model is established as follows:

[0117] Calculation of the azimuth evaluation function: For each local path trajectory of the robot, measure the first distance between the end and the end point. Determine the second distance between the start point and the end point of each path. Calculate the angle between the tangent direction of the end of each local path trajectory and the line connecting the end points. Determine the value of the azimuth evaluation function based on the above three parameters.

[0118] Design of speed evaluation function: According to the robot's motion requirements and performance limitations, a reasonable speed evaluation function is designed to evaluate the quality of paths at different speeds.

[0119] Determination of obstacle distance evaluation function: Calculate the distance between the path from each node of the robot to the next node and the obstacle, and determine the value of the obstacle distance evaluation function based on the distance.

[0120] Path adjustment: According to the trajectory evaluation model, the path trajectory from each node to the next node of the optimal path is evaluated. The comprehensive evaluation score of each path trajectory is determined by comprehensively considering the results of the azimuth evaluation function, speed evaluation function and obstacle distance evaluation function. The optimal path is adjusted according to the evaluation score and obstacle information. For example, if the obstacle distance evaluation function value of a certain path is low, it means that the path is close to the obstacle and there is a risk of collision. In this case, the path needs to be adjusted to avoid the obstacle. Repeat the above steps and continuously optimize and adjust the path until a path that meets the requirements is obtained.

[0121] It should be noted that the traditional DWA algorithm has poor obstacle avoidance effect, is prone to fall into the local optimum, and has a planned path that is too long, etc. in the process of local path planning. For example, when the robot encounters a new static obstacle on the basis of moving along the global path planning route according to the traditional DWA algorithm, the evaluation function will consider the distance between the current path and the obstacle according to the trajectory generated by the algorithm sampling, and try to escape to the area far away from the obstacle, thereby greatly deviating from the optimal path. In addition, due to the existence of the azimuth evaluation angle, the robot is more inclined to move in a straight line connecting the end direction of the current trajectory and the target point, which causes the robot to change direction only in extreme cases, and fall into the local optimal state when encountering obstacles with "convex and concave" shapes, entering a dead end, greatly reducing the success rate of obstacle avoidance. In addition, when encountering a dynamic obstacle, it collides with the obstacle due to untimely obstacle avoidance. In view of the problems of the traditional DWA algorithm, the embodiment of this specification optimizes and improves the evaluation function of the DWA algorithm. The obstacle distance evaluation function includes the minimum distance function between the end point and the static obstacle and the minimum distance function between the end point and the dynamic obstacle. The expression of the improved DWA algorithm can be:

[0122] G(v,w)=αNhead(v,w)+βvel(v,w)+γdist s (v,w)+δdist d (v,w);

[0123] Among them, Nhead(v,w) is the azimuth evaluation function, vel(v,w) is the velocity evaluation function, and dist s (v,w) is the minimum distance function between the end point and the stationary obstacle, dist d(v, w) is the minimum distance function between the end point and the dynamic obstacle, α, β, γ and δ are the preset weight values ​​of the azimuth evaluation function, the speed evaluation function, the minimum distance function between the end point and the static obstacle and the minimum distance function between the end point and the dynamic obstacle. Vel(v, ω) = |v g |, where v g is the linear velocity value of the forward simulation trajectory; dist s (v,w) and dist d (v,w) can be determined by the Euclidean distance formula.

[0124] It should be noted that the traditional DWA is a function that calculates the closest distance between the predicted trajectory and the obstacle at a given speed. The optimized obstacle distance evaluation function classifies obstacles, calculates the closest distance between the robot and dynamic obstacles and static obstacles at the end of each local path trajectory on the predicted trajectory, and finds the minimum value as the obstacle distance under this speed combination.

[0125] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0126] Improve safety: The obstacle distance evaluation function includes the minimum distance function between the end point and the static obstacle and the minimum distance function between the end point and the dynamic obstacle. By incorporating these two functions into the DWA algorithm, the distance between the robot and the obstacle during movement can be better considered, thereby reducing the risk of collision and improving the safety of robot operation.

[0127] Enhanced adaptability: The algorithm takes dynamic obstacles into account, allowing the robot to better cope with changes in the environment when planning its path. Whether facing static or dynamic obstacles, the robot can make corresponding adjustments based on the distance evaluation function, enhancing the robot's adaptability to complex environments.

[0128] Optimize path planning: By comprehensively considering the azimuth evaluation function, speed evaluation function, and obstacle distance evaluation function, and setting preset weight values ​​for each function, the robot's path selection can be more comprehensively evaluated. This can generate a more reasonable path, avoid unnecessary detours or risky behaviors, and improve the quality and efficiency of path planning.

[0129] Improved flexibility: The setting of preset weight values ​​allows the importance of each function in the algorithm to be adjusted according to actual needs and scenarios. For example, in an environment with dense obstacles, the weight of the obstacle distance evaluation function can be increased to ensure the safety of the robot; in the case of high speed requirements, the weight of the speed evaluation function can be appropriately increased. This flexibility allows the robot to better adapt to different tasks and environmental requirements.

[0130] Furthermore, the azimuth evaluation function expression is:

[0131] head(v,ω)=π-θ;

[0132] Among them, Dist NE is the first distance, Dist SE is the second distance, head(v,ω) is the azimuth function, and θ is the included angle.

[0133] It should be noted that the role of the azimuth evaluation function is to enable the robot to continuously adjust its orientation to ensure that the direction of movement is consistent with the direction of the target point and to reduce the deviation between the robot's orientation and the target point. NE / Dist SE The ratio approaches 1. The part is close to 2, and the weight of Nhead(v,w) is relatively large, so the impact on the evaluation function is relatively large. The larger the value of Nhead(v,w), the better the trajectory corresponding to the speed combination, and the higher the score of the speed combination. The speed combination with a higher azimuth evaluation function value is more likely to be selected as the next motion instruction of the robot. In other words, when the movement starts, the main purpose is to make the robot move to the target point better. But in the later stage, when it is close to the target point, the main purpose is to slow down the robot, and there is no need to make the weight of the azimuth evaluation function so large. In this process, Dist NE / Dist SE Approaching 0, Dist SE -Dist NE Gradually increasing can reduce the entire azimuth function and weaken the direction angle evaluation, thus preventing safety problems caused by changes in the robot's direction angle θ and its own steering angle ω.

[0134] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0135] Improve movement accuracy: In the early stage of robot movement, the weight of the azimuth evaluation function is relatively large, which can enable the robot to move toward the target point better, reduce the deviation between the robot's direction and the target point, and improve the accuracy of movement.

[0136] Optimize speed control: In the later stages of the movement, as the target point is approached, the azimuth function decreases and the direction angle evaluation is weakened, allowing the robot to slow down and avoid safety issues such as collisions that may be caused by high-speed movement, while also reaching the target position more accurately.

[0137] Enhanced safety: By reducing the impact of the azimuth evaluation function in the later stages of the movement, safety issues that may be caused by changes in the robot's posture are prevented, thereby improving the robot's safety throughout the entire movement process.

[0138] Adaptive adjustment: This function can make adaptive adjustments based on the distance between the robot and the target point. In the early stage of movement, it focuses on direction adjustment and rapid approach to the target, while in the later stage, it focuses on deceleration and precise position control, so that the robot's movement is more in line with actual needs.

[0139] Improve the quality of path planning: The reasonable design of the azimuth evaluation function helps to select a more appropriate speed combination as the next motion instruction of the robot, thereby optimizing the robot's motion trajectory and improving the quality of path planning.

[0140] Improve overall performance: Overall, the azimuth evaluation function can play different roles in different stages of the robot's motion, thereby improving the robot's overall motion performance, including accuracy, safety, and efficiency.

[0141] Furthermore, before obtaining the pre-generated optimal path of the robot, the embodiment of this specification obtains the optimal path of the robot through multiple iterations. Figure 2 A schematic flow chart of a robot path planning method based on an ant algorithm provided for one or more embodiments of this specification, the specific steps are as follows:

[0142] S201, when planning the path of the robot in the current iteration, obtain the current pheromone concentration and the preset heuristic function on the path updated in the previous iteration, wherein the preset heuristic function is determined based on a dynamic adjustment factor, a current node and a next node, and the dynamic adjustment factor is used to gradually weaken the influence of the preset heuristic function on the robot path planning as the number of iterations increases.

[0143] In the embodiment of this specification, before S201, a map model can be established, the ant colony algorithm parameters can be initialized, and the pheromone concentrations of the starting point and the end point can be set to be slightly higher than those of other areas. All ants are placed at the starting point, and the starting point information is placed in the taboo table. Based on the optimized and improved preset heuristic function, the next node is selected according to the state transition probability formula, and the taboo table is updated at the same time. If all ants have reached the end point, one iteration is completed.

[0144] It should be noted that the above content can be implemented through the following specific implementation plans:

[0145] Establish a map model: Use appropriate methods to build a map model of the robot's working environment, such as grid method, topology method, etc., to clarify the node and path information in the map.

[0146] Initialize the parameters of the ant colony algorithm: determine the number of ants, pheromone volatilization factor, pheromone enhancement factor and other parameters. Set the pheromone concentration at the starting point and the end point to be slightly higher than other areas to guide the ants to move to the starting point and the end point.

[0147] Place ants and update the taboo table: Place all ants at the starting point. Put the starting point information into each ant's taboo table, which is used to record the nodes that the ants have passed through to prevent the ants from visiting repeatedly.

[0148] Calculate and select the next node: Based on the optimized and improved preset heuristic function, the probability of each ant transferring from the current node to the next node is calculated according to the state transition probability formula. The ant selects the next node based on the calculated probability and adds the node information to its own taboo table. The state transition probability formula can apply existing public content, for example, the state transition probability formula is as follows:

[0149]

[0150] is the transfer probability from node i to node j, α is the pheromone factor, and its size determines the strength of the random factor of pheromone in path selection. The larger α is, the more it tends to choose a path with a higher pheromone concentration, and the weaker the random search is; β is the heuristic factor, which plays a role in guiding the search in the algorithm. If β is too small, the algorithm will fall into random search. If β is too large, the algorithm will find the optimal solution quickly, but it is easy to fall into the local optimum; τ ij (t) represents the pheromone concentration function; allowed k Represents the next movable grid set, the grid is the same as the node of the embodiment of this specification; η ij (t) is the heuristic function.

[0151] Determine whether an iteration is completed: Check the positions of all ants. If all ants have reached the end point, an iteration is completed. Pheromone update updates the pheromones on the path according to the ants' walking path. The pheromone update can be done by pheromone volatilization and pheromone enhancement. That is, on the one hand, pheromones will evaporate over time, and on the other hand, the pheromones on the path walked by the ants will be enhanced.

[0152] It should be noted that in the traditional ant colony algorithm, when the ants start from the starting position, the pheromone concentration on each path is usually very low, which will lead to a large randomness in the path selection of the ants in the initial stage. The next grid cannot be selected according to the pheromone concentration on the path, and the relationship between the starting and target grids and the current grid and the next exploration grid is not considered, which reduces the exploration rate and convergence speed. The heuristic function of this paper adds a dynamic adjustment factor and introduces a parameter composed of λ (0<λ<1) at the beginning of the path search to increase the influence of the heuristic function. As the number of iterations increases, the dynamic adjustment factor will weaken the effect of the heuristic function, thereby enhancing the global search capability and avoiding falling into the local optimum.

[0153] Further, the preset heuristic function of the embodiment of this specification is determined based on the dynamic adjustment factor, the initial node, the current node, the next node and the end point. The expression of the preset heuristic function can be The expression of the dynamic adjustment factor can be

[0154] Among them, η ij is the result of the preset heuristic function; λ is a constant, and 0<λ<1; d sj is the distance from the starting node to the next node, d ij is the distance from the current node to the next node, d jE is the distance from the next node to the end point, is the dynamic adjustment factor, N is the current number of iterations, N max is the maximum number of iterations, ε is a preset positive number, and ε is used to maintain Not 0.

[0155] It should be noted that in the traditional ant colony algorithm, due to the influence of local pheromones, the starting node and the target node are not considered from a global perspective during the path search process, and only the current node and the next node are focused on, which may result in the path of the next node being selected that deviates from the direction of the target node. Therefore, the embodiment of this specification considers the entire search process, increases the connection between the target node and the next node, and amplifies the influence of the ant's selection of the next node on the entire state transition.

[0156] d sj is the distance from the starting node to the next node. It is meaningless to calculate the distance between the current node and the starting node at this moment, because the current node is the action that has been selected at the previous moment. It can only indicate the distance between the current node and the starting node, and cannot indicate the relationship between the grid to be selected next and the starting point and the target point. Therefore, d is selected. sj Better increase the connection between the starting node and the next node.

[0157] d jEIndicates the distance between the next node and the end point, which can better estimate whether the next node is closer to the target point, because the purpose of the embodiment of this specification is to select the shortest path from the starting node to the end point.

[0158] λ can be adjusted manually with the number of iterations. In the early stage of iteration, λ starts to increase from 0, and 1 / λ is large, which is a balanced heuristic function (because d jE In the early search conditions of the iteration, λ is set to increase the influence of the heuristic function.

[0159] Dynamic adjustment factor φ: In the early stage of the algorithm, due to the small number of iterations, φ approaches 1. As the number of iterations increases, the dynamic adjustment factor will decrease. At this time, 1 / λ can also be gradually increased to enhance the global search capability and avoid falling into the local optimum.

[0160] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0161] Global perspective consideration: Through the improved heuristic function, the connection between the end point and the next node is increased, so that the starting node and the end point can be considered from a global perspective during the path search process, avoiding only focusing on the current node and the next node and choosing a path that deviates from the end point, thereby improving the globality and accuracy of path planning.

[0162] Enhance the connection between the starting point and the next node: Select the distance between the starting node and the next node to better increase the connection between the starting node and the next node. It can indicate the relationship between the node to be selected next and the starting node and the end point, which helps to find a better path.

[0163] Approaching the goal more accurately: The introduction of the distance between the next node and the end point can better estimate whether the next node is closer to the end point, which helps to achieve the goal of selecting the shortest path from the starting node to the end point.

[0164] Enhanced global search capability: The dynamic adjustment factor is close to 1 at the beginning of the algorithm and gradually decreases as the number of iterations increases. In the early stage of the algorithm, the strong influence of the heuristic function helps to quickly explore the path; as the iterations proceed, the dynamic adjustment factor gradually decreases, which can enhance the global search capability, avoid falling into the local optimum, and increase the possibility of finding the optimal path.

[0165] Ensure algorithm stability: To prevent the dynamic adjustment factor from being equal to 0 in the later stage of iteration, causing the heuristic function to become 0, a very small positive number is added to ensure the effectiveness of the heuristic function and improve the stability and reliability of the algorithm.

[0166] S202, determining the pheromone concentration change corresponding to the robot moving from each node to the next node according to the current pheromone concentration.

[0167] In the embodiments of this specification, regarding the above S202, the following specific implementation scheme can be adopted:

[0168] Data collection and preparation: Determine the robot's environment and related node information. Collect pheromone concentration data for each node.

[0169] Establish a pheromone concentration update model: Select a suitable algorithm or model to update the pheromone concentration. For example, you can use the pheromone update rule in the ant colony algorithm. Consider factors that affect the change of pheromone concentration, such as path length, obstacles on the path, and the robot's motion history.

[0170] Calculate the pheromone concentration of the next node: For each node, calculate the change in pheromone concentration from the node to the next node according to the selected model and the current pheromone concentration.

[0171] S203, determining the heuristic parameters corresponding to the robot's transition from each node to the next node according to the preset heuristic function.

[0172] In the embodiments of this specification, regarding the above S203, the following specific implementation scheme can be adopted:

[0173] Collect node information: Collect information about each node in the robot's environment. This information may include the location of the node, its connection with other nodes, and the characteristics of the node (such as whether it is an obstacle, a target point, etc.).

[0174] Calculate heuristic parameters: For each node, consider the situation to the next node. Substitute the relevant information of the current node and the next node into the preset heuristic function and calculate to determine the heuristic parameters. All nodes can be traversed in a certain order, and the heuristic parameters of the next node of each node are calculated.

[0175] S204, determining the first transition probability of the robot from each node to the next node in the current iteration according to the change in pheromone concentration corresponding to each node to the next node and the heuristic parameter corresponding to each node to the next node.

[0176] In the embodiments of this specification, regarding the above S204, the following specific implementation scheme can be adopted:

[0177] Calculate the first transition probability: For each node, consider its situation to the next node separately. Use the above state transition probability formula or other general state transition probability formula of ant colony algorithm to calculate the first transition probability of the robot from each node to the next node.

[0178] Calculate the first transition probability of each node in turn: Calculate the first transition probability of each node to its next node according to the above state transition probability formula or other general state transition probability formulas of ant colony algorithms. Traverse all nodes to ensure that the first transition probability of each node is accurately calculated.

[0179] S205, after generating the planned path of the robot in the current iteration based on the first transition probability, updating the current pheromone concentration through a pheromone update function to obtain the latest pheromone concentration on the path.

[0180] In the embodiments of this specification, regarding the above S205, the following specific implementation scheme can be adopted:

[0181] Generate planned path: Generate a planned path for the robot in the current iteration based on the first transition probability determined previously.

[0182] Determine the pheromone update function: clarify the specific form and parameters of the pheromone update function used. This function usually considers factors such as path length and path quality to determine the amount of pheromone update.

[0183] Traverse the planned path: According to the generated planned path, traverse each node and the connection between nodes on the path in turn.

[0184] Update pheromone concentration: For each connection on the path (from one node to the next), use the pheromone update function to calculate the pheromone update amount for that connection. Combine the original pheromone concentration with the calculated update amount to get the latest pheromone concentration for that connection on the path.

[0185] Complete update: After traversing the entire planned path, the pheromone concentrations of all connections on the path are updated, and the latest pheromone concentrations on the path are obtained.

[0186] Furthermore, since the ant colony algorithm requires different volatility factors to adjust the increase or decrease of pheromones at different stages, the volatility factor is improved to achieve dynamic adjustment. The improved dynamic volatility factor is added to the pheromone update function, that is, the current pheromone concentration is updated through the pheromone update function. When the latest pheromone concentration on the path is obtained, the pheromone update function τ is used to update the current pheromone concentration. ij (t+1)=(1-ρ(N c +1))τ ij (t)+ρ(N c +1)*Δτ ij (t) Update the current pheromone concentration to obtain the latest pheromone concentration on the path. ij (t+1) is the latest pheromone concentration on the path from the current node i to the next node j, τ ij(t) is the current pheromone concentration on the path from the current node i to the next node j, ρ(N c +1) is the current pheromone volatilization factor, ρ(N c ) is the last pheromone volatilization factor, Δτ ij (t) is the increase in the amount of pheromone released on the path from the current node i to the next node j.

[0187] It should be noted that the dynamically adjusted volatility factor improved in the embodiment of this specification is no longer a constant, but changes with the increase of the number of iterations. Dynamically adjust the value of the volatilization factor to adjust the pheromone on the path and volatilize more pheromones on the poorer paths, so that ants can avoid repeatedly exploring poorer paths, explore more and better paths, and improve the convergence time of the algorithm. cmax is the maximum number of iterations, N c is the current iteration number.

[0188] It should be noted that the dynamic adjustment of the volatility factor changes with the increase in the number of iterations. In the early stage of the algorithm, in order to allow the ants to better explore the optimal path and not be disturbed by the path they have already walked, the volatility factor value is set to a larger value. When the algorithm starts running, the value of the volatility factor is dynamically adjusted according to the search status and performance of the algorithm to achieve a better search effect. Therefore, the embodiment of this specification controls the volatility factor to change according to the change in the number of iterations. As the number of iterations increases, the volatility factor gradually changes. ρ mi The value of n can be set as a parameter in the initial stage of the algorithm according to the needs of the algorithm. In order to ensure that the algorithm can run normally and has a certain global search capability, the minimum value of the volatility factor will be set to a small positive number to avoid the algorithm completely losing randomness and global search capability due to the value of 0. m in can take the value of 0.1.

[0189] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0190] Promote path exploration: In the early stage of the algorithm, setting a larger pheromone volatilization factor can make the pheromones on the path previously walked by ants evaporate faster, reduce interference with subsequent ants, and allow ants to better explore new possible paths, increasing the chance of finding the optimal path.

[0191] Dynamic optimization search: According to the algorithm's search status and performance, the value of the volatility factor is dynamically adjusted through the formula, which enables the algorithm to adopt different strategies at different search stages. In the early stage, extensive exploration is encouraged, and in the later stage, more emphasis is placed on strengthening existing good paths, so as to achieve better search results.

[0192] Guarantee algorithm performance: Setting the minimum value of the volatility factor to a small positive number (such as 0.1) can ensure that the algorithm always has a certain degree of randomness and global search capabilities during operation. This avoids the situation where the algorithm may rely entirely on previous path information when the volatility factor is 0, resulting in falling into the local optimum and being unable to find the global optimal path, thereby improving the reliability and stability of the algorithm.

[0193] Improve search efficiency: By reasonably adjusting the pheromone volatilization factor, the algorithm can explore the search space more effectively, reduce unnecessary repeated searches and the selection of wrong paths, thereby improving search efficiency and finding the optimal or near-optimal path more quickly.

[0194] Furthermore, in order to improve the convergence speed, the embodiments of this specification classify the paths taken by the ants into better paths and worse paths, and increase or decrease the pheromone according to the better or worse degree. Before the pheromone is iteratively updated, the path length is compared with the known path data to find the shortest path D in the current state. best and the longest path D worst The formula for increasing the amount of pheromone released on the path from the current node i to the next node j is: Among them, D worst is the longest path from the current node i to the next node j, D best is the shortest path from the current node i to the next node j, D k is the current path from the current node i to the next node j.

[0195] Further, the embodiment of this specification describes that according to the pheromone update function τ ij (t+1)=(1-ρ(N c +1))τ ij (t)+ρ(N c +1)*Δτ ij (t) When updating the current pheromone concentration, if Then D best <D k <D worst , indicating that the current path is not the longest or shortest path, and the iterative search continues; if Then D k >D worst or D k <D best If D k >D worst , indicating that the current path is longer than the longest path before, then the current path replaces the previous longest path to become the longest path in the next iteration, and increases the volatility factor of the path to reduce the pheromone on the path; if D k<D best , indicating that the current path is shorter than the previous shortest path, then the current path replaces the previous shortest path to become the shortest path in the next iteration, and reduces the volatility factor of the path, so that the pheromone on the path increases.

[0196] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0197] Continuous optimization: By continuously determining whether the current path is the longest or shortest path, and continuing to iterate when it is not, it helps to find a better path solution.

[0198] Update the longest path: When the current path is found to be longer than the previous longest path, it is replaced with the longest path in the next iteration, and the volatility factor of the path is increased to reduce the pheromone. This can prevent subsequent searches from being overly focused on this longer path that may not be optimal, and encourage the algorithm to continue to explore better paths.

[0199] Update the shortest path: When the current path is found to be shorter than the previous shortest path, it is replaced as the shortest path in the next iteration, and the volatility factor of the path is reduced to increase the pheromone. This helps to strengthen this better shorter path and increase the probability of the algorithm selecting this path in subsequent searches, thereby finding a shorter path faster.

[0200] Balanced search: By dynamically updating the longest and shortest paths and adjusting the corresponding volatility factors, balanced control of the search process is achieved. This not only avoids the algorithm from converging to the local optimal longer path too early, but also strengthens the exploration of better shorter paths, increasing the possibility of finding the global optimal path.

[0201] Improve efficiency: This mechanism of dynamically adjusting pheromones and volatility factors according to path length enables the algorithm to search more targetedly, reduce unnecessary calculation and search time, and improve the overall efficiency of the algorithm.

[0202] Furthermore, in order to prevent the algorithm from being stuck in a local optimum due to excessively high pheromone concentration in some areas, upper and lower limits of pheromone are set, i.e., pre-set pheromone concentration thresholds. When the current pheromone concentration is updated by the pheromone update function to obtain the latest pheromone concentration on the path, the pre-set pheromone concentration threshold is obtained, and the pheromone concentration threshold is used to avoid the local optimum solution of path planning; the current pheromone concentration is updated by the pheromone update function to obtain the pheromone concentration to be updated; the pheromone concentration to be updated is processed based on the pheromone concentration threshold to obtain the latest pheromone concentration on the path.

[0203] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0204] Avoiding local optimal solutions: By setting the pheromone concentration threshold, you can avoid the situation of local optimal solutions in the path planning process. This helps the algorithm escape the trap of local optimal solutions during the search process and is more likely to find the global optimal path.

[0205] Improve search efficiency: Avoiding falling into local optimal solutions can reduce the search time and computing resource consumption on invalid paths, thereby improving the search efficiency of the entire path planning.

[0206] Enhanced robustness of the algorithm: With the constraint of the pheromone concentration threshold, the algorithm can perform path planning more stably when facing different problems and scenarios, and is less susceptible to local optimal situations, which enhances the robustness of the algorithm.

[0207] Obtain better path planning results: By avoiding local optimal solutions, the algorithm can search the path space more comprehensively, thus having a greater chance of finding better path planning results and improving the quality and practicality of the path.

[0208] Improving the adaptability of the algorithm: The setting of pheromone concentration threshold can make the algorithm better adapt to different environments and problem conditions, thereby improving the adaptability and flexibility of the algorithm.

[0209] Furthermore, when the pheromone concentration to be updated is processed based on the pheromone concentration threshold to obtain the latest pheromone concentration on the path, the formula Process the pheromone concentration to be updated to obtain the latest pheromone concentration on the path, where the pheromone concentration threshold is [τ min ,τ max ],τ min is the minimum pheromone concentration, τ max is the maximum pheromone concentration.

[0210] S206, based on the latest pheromone concentration and the preset heuristic function, determine the second transition probability of the robot from each node to the next node in the next iteration, generate the planned path of the robot in the next iteration based on the second transition probability, and obtain the optimal path of the robot after the last iteration.

[0211] In the embodiments of this specification, regarding the above S206, the following specific implementation scheme can be adopted:

[0212] Determine the second transition probability: For each node, consider its situation to the next node. Combine the latest pheromone concentration with the preset heuristic function and use the same calculation method as the first transition probability to determine the second transition probability.

[0213] Generate a planned path: Based on the calculated second transition probability, determine the planned path for the next iteration for the robot. The movement of the robot from one node to the next node can be determined by random selection based on the second transition probability, thereby generating a complete planned path.

[0214] Repeat iteration: Follow the above steps for multiple iterations. Each iteration determines the second transition probability of the next iteration based on the pheromone concentration updated in the previous iteration, and generates the corresponding planning path.

[0215] Determine the optimal path: After completing the last iteration, compare the planned paths generated in all iterations. According to the preset optimization criteria (such as shortest path length, shortest time, etc.), select the best path from all planned paths as the final optimal path of the robot.

[0216] It should be noted that the embodiments of this specification have the following beneficial effects regarding the above content:

[0217] Improve path planning efficiency: By presetting the heuristic function and pheromone concentration to determine the robot's transfer probability from each node to the next node, the robot can be guided to find a better path more quickly, reducing unnecessary searches and calculations, thereby improving the efficiency of path planning.

[0218] Adaptive adjustment: The introduction of dynamic adjustment factors makes the influence of the preset heuristic function on the robot's path planning gradually weaken as the number of iterations increases. This means that in the initial stage, the heuristic function can help the robot explore potential paths faster, and as the iterations proceed, the effect of pheromone concentration gradually increases, making path planning more dependent on actual search experience and improving the adaptive ability of path planning.

[0219] Optimize path quality: Determine the transfer probability based on pheromone concentration and heuristic parameters, and comprehensively consider the historical experience of the path and the current heuristic information to generate a better planning path. By continuously iterating and updating the pheromone concentration, the path quality is continuously optimized, and the optimal path is finally obtained.

[0220] Enhanced robustness: Through multiple iterations and pheromone updates, this method can find a suitable path under different environmental and task conditions. Even in the face of complex environments and uncertain factors, it can adapt to changes by continuously adjusting the transition probability, which enhances the robustness of the robot's path planning.

[0221] Improve solution accuracy: Through continuous iteration and optimization of transfer probability, the robot can find the optimal path more accurately, reduce path errors, and improve the solution accuracy of path planning.

[0222] It should be noted that the traditional ant colony algorithm used for path planning of mobile robots in grid environments has problems such as slow convergence speed, weak dynamic planning ability and easy to fall into local optimum. The embodiment of this specification improves the pheromone update method by adding a dynamic adjustment factor to the ant colony algorithm to optimize the heuristic function; classifies the paths taken by ants; introduces an adaptive volatility factor to dynamically adjust its update strategy; and realizes global path optimization of mobile robots. At the same time, a redundant point deletion strategy can be used to smooth the planned path, so that the robot can reduce redundant node searches and reduce turning angles during the path planning process, thereby speeding up the search efficiency.

[0223] It should be noted that the paths planned by traditional algorithms usually have many redundant points, many turns, and sharp turning points. In order to remove unnecessary turning points, reduce the number of turns and improve the smoothness of the path, the embodiment of this specification adopts a redundant point removal strategy for optimization processing in the path smoothing part. The redundant point removal steps are as follows:

[0224] (1) Determine the co-linearity of each node on the path. If there are three or more nodes on one line, remove the redundant nodes and only keep the first and last nodes.

[0225] (2) Delete redundant turning points. Traverse all remaining nodes on the path. If there is no obstacle on the line between any node and the two nodes before and after it, delete the node.

[0226] It should be noted that the redundant points can be deleted through the following specific implementation scheme:

[0227] Collinearity judgment and redundant node deletion: Select the nodes on the path one by one, starting from the third node. For the currently selected node and its first two nodes, determine whether the three nodes are collinear. You can use the vector calculation method to determine that if the three nodes are collinear, delete the middle node and only keep the first and last nodes. According to the above method, traverse and judge all nodes on the path to complete the processing of collinear nodes.

[0228] Delete redundant turning points: Traverse all remaining nodes on the path after the first step. For the currently traversed node, check whether there are obstacles on the line between the node and the two nodes before and after it. You can compare it with the environment model to determine whether there are obstacles on the line. If there are no obstacles on the line, it means that the node is a redundant turning point and it will be deleted from the path. Continue to traverse the next node until all nodes have been processed.

[0229] Figure 3A schematic structural diagram of a robot path adjustment device based on a DWA algorithm provided for one or more embodiments of this specification includes: a path acquisition unit 301 , a target point setting unit 302 , a parameter setting unit 303 and a path adjustment unit 304 .

[0230] A path acquisition unit 301 acquires a pre-generated optimal path of the robot;

[0231] A target point setting unit 302 is configured to set the turning point of the optimal path as the end of each local path trajectory;

[0232] A parameter setting unit 303 is used to set the parameter information of the robot based on the optimal path, wherein the parameter information includes angle and speed values;

[0233] The path adjustment unit 304 adjusts the optimal path according to a preset trajectory evaluation model and obstacle information to obtain an adjusted path. The trajectory evaluation model evaluates the path trajectory of the robot from each node to the next node based on the DWA algorithm. The DWA algorithm includes an azimuth evaluation function, a speed evaluation function and an obstacle distance evaluation function. The azimuth evaluation function is determined based on a first distance between the end of each local path trajectory and the end point, a second distance between the starting point and the end point of each path, and an angle between the tangent direction of the end of each local path trajectory and the line connecting the end point.

[0234] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

Claims

1. A robot path adjustment method based on DWA algorithm, It is characterized in that include: Get the optimal path of the pre-generated robot; The turning points based on the optimal path are set as the ends of the trajectories of each local path; Setting parameter information of the robot based on the optimal path, the parameter information including angle and speed values; The optimal path is adjusted according to a preset trajectory evaluation model and obstacle information to obtain an adjusted path. The trajectory evaluation model evaluates the path trajectory of the robot from each node to the next node based on the DWA algorithm. The DWA algorithm includes an azimuth evaluation function, a speed evaluation function and an obstacle distance evaluation function. The azimuth evaluation function is determined based on a first distance between the end of each local path trajectory and the end point, a second distance between the starting point and the end point of each path, and an angle between the tangent direction of the end of each local path trajectory and the line connecting the end point.

2. The method according to claim 1, It is characterized in that Before obtaining the pre-generated optimal path of the robot, the method further includes: When planning the path of the robot in the current iteration, the current pheromone concentration and the preset heuristic function on the path updated in the previous iteration are obtained, and the preset heuristic function is determined based on the dynamic adjustment factor, the current node and the next node. The dynamic adjustment factor is used to gradually weaken the influence of the preset heuristic function on the robot path planning as the number of iterations increases; Determine the pheromone concentration change amount corresponding to the robot moving from each node to the next node according to the current pheromone concentration; Determine the heuristic parameters corresponding to the robot's transition from each node to the next node according to the preset heuristic function; Determine the first transition probability of the robot from each node to the next node in the current iteration according to the change in pheromone concentration corresponding to each node to the next node and the heuristic parameter corresponding to each node to the next node; After generating the planned path of the robot in the current iteration based on the first transition probability, updating the current pheromone concentration through the pheromone update function to obtain the latest pheromone concentration on the path; Based on the latest pheromone concentration and the preset heuristic function, the second transition probability of the robot from each node to the next node in the next iteration is determined, the planned path of the robot in the next iteration is generated based on the second transition probability, and the optimal path of the robot is obtained after the last iteration.

3. The method according to claim 1, It is characterized in that The obstacle distance evaluation function includes the minimum distance function between the end point and the static obstacle and the minimum distance function between the end point and the dynamic obstacle; The DWA algorithm expression is: G(v,w)=αNhead(v,w)+βvel(v,w)+γdist s (v,w)+δdist d (v,w); Among them, Nhead(v,w) is the azimuth evaluation function, vel(v,w) is the velocity evaluation function, and dist s (v,w) is the minimum distance function between the end point and the stationary obstacle, dist d (v,w) is the minimum distance function between the end point and the dynamic obstacle, α, β, γ and δ are the preset weight values ​​of the azimuth evaluation function, the speed evaluation function, the minimum distance function between the end point and the static obstacle and the minimum distance function between the end point and the dynamic obstacle, respectively.

4. The method according to claim 3, It is characterized in that The azimuth evaluation function expression is: head(v,ω)=π-θ; Among them, Dist NE is the first distance, Dist SE is the second distance, head(v,ω) is the azimuth function, and θ is the included angle.

5. The method according to claim 2, It is characterized in that The preset heuristic function is determined based on the dynamic adjustment factor, the initial node, the current node, the next node and the end point. The expression of the preset heuristic function is: The expression of the dynamic adjustment factor is Among them, η ij is the result of the preset heuristic function; λ is a constant, and 0<λ<1; d sj is the distance from the starting node to the next node, d ij is the distance from the current node to the next node, d jE is the distance from the next node to the end point, is the dynamic adjustment factor, N is the current number of iterations, N max is the maximum number of iterations, ε is a preset positive number, and ε is used to maintain Not 0.

6. The method according to claim 2, It is characterized in that The updating of the current pheromone concentration by the pheromone updating function to obtain the latest pheromone concentration on the path includes: According to the pheromone update function τ ij (t+1)=(1-ρ(N c +1))τ ij (t)+ρ(N c +1)*Δτ ij (t) updating the current pheromone concentration to obtain the latest pheromone concentration on the path, Among them, τ ij (t+1) is the latest pheromone concentration on the path from the current node i to the next node j, τ ij (t) is the current pheromone concentration on the path from the current node i to the next node j, ρ(N c +1) is the current pheromone volatilization factor, ρ(N c ) is the last pheromone volatilization factor, Δτ ij (t) is the increase in the amount of pheromone released on the path from the current node i to the next node j, N cmax is the maximum number of iterations, N c is the current iteration number.

7. The method according to claim 6, It is characterized in that The formula for increasing the amount of pheromone released on the path from the current node i to the next node j is: Among them, D worst is the longest path from the current node i to the next node j, D best is the shortest path from the current node i to the next node j, D k is the current path from the current node i to the next node j.

8. The method according to claim 7, It is characterized in that The pheromone update function τ ij (t+1)=(1-ρ(N c +1))τ ij (t)+ρ(N c +1)*Δτ ij (t) updating the current pheromone concentration, comprising: like Then D best <D k <D worst , indicating that the current path is not the longest or shortest path, and the iterative search continues; like Then D k >D worst or D k <D best ; If D k >D worst , indicating that the current path is longer than the longest path before, then the current path replaces the previous longest path to become the longest path in the next iteration, and increases the volatility factor of the path to reduce the pheromone on the path; if D k <D best , indicating that the current path is shorter than the previous shortest path, then the current path replaces the previous shortest path to become the shortest path in the next iteration, and reduces the volatility factor of the path, so that the pheromone on the path increases.

9. The method according to claim 2, It is characterized in that The updating of the current pheromone concentration by the pheromone updating function to obtain the latest pheromone concentration on the path includes: Obtaining a preset pheromone concentration threshold, where the pheromone concentration threshold is used to avoid a local optimal solution in path planning; Update the current pheromone concentration by using a pheromone update function to obtain a pheromone concentration to be updated; The pheromone concentration to be updated is processed based on the pheromone concentration threshold to obtain the latest pheromone concentration on the path.

10. A robot path adjustment device based on DWA algorithm, It is characterized in that include: A path acquisition unit, which acquires the optimal path of the robot generated in advance; A target point setting unit, which sets the turning point of the optimal path as the end of each local path trajectory; A parameter setting unit, which sets parameter information of the robot based on the optimal path, wherein the parameter information includes angle and speed values; The path adjustment unit adjusts the optimal path according to a preset trajectory evaluation model and obstacle information to obtain an adjusted path. The trajectory evaluation model evaluates the path trajectory of the robot from each node to the next node based on the DWA algorithm. The DWA algorithm includes an azimuth evaluation function, a speed evaluation function and an obstacle distance evaluation function. The azimuth evaluation function is determined based on a first distance between the end of each local path trajectory and the end point, a second distance between the starting point and the end point of each path, and an angle between the tangent direction of the end of each local path trajectory and the line connecting the end point.

Citation Information

Cited By

  • Route planning method for narrow scene and related equipment

    CN119665974A

  • Robot deceleration obstacle avoidance method based on DWA

    CN119759019A