Hybrid path optimization method for mobile robot in complex dynamic environment

Abstract

The invention belongs to the technical field of mobile robot navigation, and particularly relates to a mobile robot mixed path optimization method in a complex dynamic environment, which comprises the following steps: firstly, a system calculates a steering angle and a moving speed vector of a robot based on a fuzzy control algorithm according to a target point direction and an obstacle distance; and then, detecting whether the robot is caught in an unknown deadlock area or not through a weighted average speed # imgabs0 #, and completing a navigation task. According to the method, real-time path adjustment based on the target direction and the obstacle distance is achieved by introducing the fuzzy control algorithm, the adaptive capacity to the dynamic environment is improved, the deadlock area is dynamically stored and efficiently inquired by constructing the quadtree data structure, the robot can rapidly recognize and avoid the known deadlock area, and therefore the robot can rapidly recognize and avoid the deadlock area. And meanwhile, an unknown deadlock area is identified by utilizing a speed-based detection method, and automatic deadlock removal is realized through an adjustment strategy, so that the dependence on environmental information is reduced, and the robustness and navigation efficiency of the system are remarkably improved.

Description

Technical Field

[0001] The invention relates to the technical field of mobile robot navigation, and in particular to a hybrid path optimization method for a mobile robot in a complex dynamic environment. Background Art

[0002] With the rapid development of artificial intelligence and automation technology, autonomous navigation technology of mobile robots has been widely used in many fields such as industrial manufacturing, logistics and transportation, medical services, agricultural production, etc., playing a vital role. Path planning, as the core technology of autonomous navigation system, directly affects the operation efficiency, safety and system reliability of robots in various mission scenarios. Therefore, efficient path planning algorithms have become a key factor in promoting the development of robotics technology.

[0003] At present, the widely used classical path planning algorithms include A* algorithm, Dijkstra algorithm and rapidly expanding random tree (RRT) algorithm. These algorithms usually show good path optimization capabilities in static and structured environments, and can effectively find the optimal or suboptimal path. However, in complex and dynamic environments, the performance of these algorithms is insufficient. The main reason is that traditional algorithms are difficult to deal with the interference of dynamic obstacles and uncertain factors in the environment. In addition, these algorithms are usually based on the perfect assumption of sensor input data, while in practical applications, sensor data is often accompanied by noise and uncertainty, which seriously affects the stability and reliability of path planning. What's more serious is that in complex environments (such as U-shaped obstacles), traditional algorithms are prone to fall into local optimal solutions and it is difficult to find the global optimal path, which limits their promotion in practical applications. Summary of the invention

[0004] 1. Technical issues to be resolved In view of the deficiencies of the prior art, the present invention provides a hybrid path optimization method for mobile robots in complex dynamic environments, which can effectively avoid and handle deadlock problems in unknown and complex environments. By introducing a fuzzy control algorithm, real-time path adjustment based on target direction and obstacle distance is achieved, and the adaptability to dynamic environments is improved. In addition, by constructing a quadtree data structure to dynamically store and efficiently query deadlock areas, the robot can quickly identify and avoid known deadlock areas, and use a speed-based detection method to identify unknown deadlock areas, and automatically get out of trouble by adjusting strategies. The present invention not only reduces the dependence on environmental information, but also significantly improves the robustness and navigation efficiency of the system, thereby ensuring that the robot can autonomously complete navigation tasks in unstructured environments. It has important theoretical significance and practical application value, and solves the problem of easy deadlock in unknown and complex environments.

[0005] (II) Technical solution In order to achieve the above-mentioned purpose, the present invention specifically adopts the following technical solutions: Known conditions: Maximum number of system iterations , time step , robot speed , speed threshold , distance threshold , the robot's historical iteration position set and speed set ; Explanation of known conditions and variables to be determined: first, Indicates the maximum number of iterations of the system. When the maximum number of iterations is reached, the system will stop executing the navigation algorithm to prevent the algorithm from falling into an infinite loop. is the time step, which is kept fixed in each iteration to ensure stability and accuracy during the calculation process. The robot will move at a constant speed in the navigation based on fuzzy control. Move in the calculated direction at a constant speed is a preset constant. Speed ​​threshold Used to determine whether the robot is in a deadlock state. Tolerance distance Used to determine whether the robot has reached the target point, that is, when the distance between the robot and the target point is less than When , the system will consider that the robot has successfully reached the target, thus terminating the execution of the navigation algorithm. and collection It is used to record all the position and speed information of the robot in historical iterations and initialize it to an empty set. Indicates The position at the iteration, Indicates The speed of the robot under iterations. It is a sequence of all coordinate points of the robot from the starting point to the target point.

[0006] A hybrid path optimization method for a mobile robot in a complex dynamic environment comprises the following steps: First, based on the fuzzy control algorithm, the system calculates the robot's steering angle and moving speed vector according to the target point direction and obstacle distance; Then, by weighted average speed Detect if the robot is stuck in an unknown deadlock area; If the robot is detected to be in deadlock, a circular area covering the current position is generated and stored in the deadlock area list of the quadtree structure. At the same time, the speed is adjusted according to the deadlock escape strategy. , so that the robot can quickly get out of trouble; If deadlock is not detected, the speed set by the fuzzy control algorithm is used. keep going; When the navigation termination condition is reached, the system outputs the navigation path , complete navigation tasks, ensure the robot's autonomous navigation capabilities in complex environments, and effectively avoid known and unknown deadlock areas.

[0007] Furthermore, the robot navigation path , the specific steps to obtain are as follows: Step 1: Construct a fuzzy controller as follows: (1) Determine the input and output variables: The fuzzy controller has a four-input and one-output structure; The input variables include: the angle between the target point direction and the robot's current direction , the unit is radian (rad), the range is ; A positive value indicates that the target is on the right side of the robot, and a negative value indicates that the target is on the left side; The distances to obstacles on the left, front, and right sides of the robot , , , the unit is meter (m), the range is ; The output variable is the steering angle that the robot should adjust , the unit is radian (rad), the range is ; Among them, positive values ​​indicate a right turn, and negative values ​​indicate a left turn; (2) Setting fuzzy sets: For the target direction , set its fuzzy set to ; Distance to obstacles , , , set its fuzzy set to ; For output variables , set its fuzzy set to ; (3) Setting the membership function: In the membership function setting of each variable, the generalized bell-shaped membership function is selected as the membership function of the input variable and the output variable; The reason is that the generalized bell-shaped membership function has a smooth membership change characteristic and can be used when the input value is close to the central value. Achieve smooth transition; In addition, by adjusting its parameters and , the shape and width of the membership function can be flexibly changed to adapt to different control requirements, thereby improving the response stability and flexibility of the system; The membership function is expressed as: (28) Among them, the parameters Control the width of the curve, parameters Controls the steepness of the curve, parameter Control the center position of the curve; In the input membership function, the target direction The parameters of the three membership functions They are [1.16, 3.28, -2.68; 0.89, 3.04, 0; 1.26, 3.28, 2.79] respectively; The parameters of the two membership functions for the three distance input variables are the same. They are [0.215, 4.77, 0.39; 0.211, 4.703, 0.899] respectively; In the output membership function, the steering angle The parameters of the five membership functions They are [0.76, 4.04, -3.07; 0.60, 4.05, -1.44; 0.66, 3.62, 0; 0.61, 4.09, 1.40; 0.82, 4.05, 3.14] respectively; (4) Designing fuzzy rules: By defining a fuzzy rule table, the robot can dynamically adjust its direction based on the target direction and obstacle distance, thereby avoiding obstacles and moving toward the target. A total of 24 rules are defined, and the rules are in the following format: (29) in, , , , The input variables are , , , In the The fuzzy set under the rules, is the fuzzy set of output variables, Iterates through all the rules The specific fuzzy control rules are shown in the following table: ; Here is an explanation of the first fuzzy rule: Rule 1: If the target direction is "leftward" and the left distance is "far" and the front distance is "far" and the right distance is "far", the turning angle is "left forward turn"; The fuzzy control rule generation logic is divided into two stages: first, the optimal initial direction is selected according to the distance of the obstacle. The robot will move in the direction farthest from the obstacle first to ensure safe obstacle avoidance; second, on this basis, the target point direction is adjusted so that the robot can move towards the target as much as possible while avoiding obstacles. The final steering angle is dynamically adjusted by combining the initial obstacle avoidance direction with the offset of the target point direction to ensure that the robot can avoid obstacles efficiently and maintain goal orientation.

[0008] Step 2: Construct a deadlock list as follows: Introducing the deadlock list ( ) Record known deadlock areas. Its main purpose is to prevent the robot from getting stuck in a loop while moving toward the target point and to avoid it from repeatedly entering the detected deadlock area. By introducing a deadlock list , which can effectively improve the efficiency and reliability of navigation algorithms; Deadlock List The quadtree structure is used for storage and search. The quadtree can efficiently manage and retrieve spatial data, making it faster to make regional judgments in complex environments. Each node of the quadtree represents a rectangular area, which is defined as: (30) This rectangular area is defined by the lower left corner coordinates and the upper right corner coordinates OK, a quadtree is constructed by recursively dividing a rectangular region into four sub-regions (quadrants), each of which is itself a quadtree node; The principle of division is along Axis and The midpoints of the axes divide the current rectangular area into four sub-rectangles of equal size: (31) in: (32) The system first initializes an empty quadtree to store the boundaries of the circular deadlock area. At the same time, the root node boundary of the quadtree is set to the range of the entire workspace, thereby ensuring that the robot can dynamically update and query the deadlock area during navigation.

[0009] Step 3: Fuzzy control path planning is as follows: In fuzzy control path planning, the system uses the input fuzzy variables, the predefined membership function and the fuzzy rule table, and the Min-Max inference rule to obtain the output variables. Membership degree: (33) in, , , , The input variables are , , , In the The fuzzy set under the rules, is the output variable fuzzy set of ; is the membership function, Iterates through all the rules The formula first calculates the membership value of each input variable, and then The function gets the matching degree of each rule, and then uses The function takes the maximum value from all rules to determine the membership of the output variable; The output variable is obtained in fuzzy inference After the membership function is calculated, the fuzzy membership value needs to be converted into a specific numerical output to determine the steering angle that the robot should adjust. , the centroid method is used for defuzzification. Its principle is to take the centroid of the area enclosed by the fuzzy membership function curve and the horizontal coordinate as the final output value of fuzzy reasoning, that is: (34) in, is the possible value of the output variable, It is the membership degree under different values. The numerator represents the weighted product of each value and its membership degree, and the denominator is the sum of the membership degrees. The result is the final output value of fuzzy reasoning. , is the weighted average of all possible values; Based on the robot's current direction and the inferred steering angle , calculate the direction angle of the velocity vector for: (35) (36) in, is the current position, is the position at the last moment, and the two are used to calculate the current direction angle of the robot ; The robot follows the direction At speed When moving, its velocity vector on the two-dimensional plane is It is expressed as: (37) in, and Respectively indicate the speed and The weight in direction, is the preset uniform speed of the robot.

[0010] Step 4: Calculate the weighted average speed The details are as follows: To determine if the robot is trapped in an area, use the weighted average speed To determine whether the robot is trapped by an obstacle; The recursive formula of weighted velocity is shown as follows: (38) in, It is The weighted average speed of iterations, It is The weighted average speed of iterations, is the speed of the first iteration, It is The actual speed of the iterations, is the dynamic weight attenuation coefficient, which determines the influence of the current speed on the weighted average speed; The specific expressions are as follows: (39) in, Indicates the change between the current speed and the last weighted average speed, reflecting the amplitude of speed fluctuation. is the modulus of the current velocity, plus a small constant To prevent the denominator from being zero, and Function ensures Restricted to within the scope; When the speed changes greatly, The value increases, which means that the influence of the current actual speed on the weighted average speed increases, and it can respond to speed changes more quickly; when the speed is relatively stable, The value decreases, relying more on historical speed data, and obtaining a smoother weighted average speed; By recursive formula (11), we can It is expanded into the weighted sum of all historical velocities, as shown in the following equation (13): (40) in, is the current iteration number, is the index of a historical moment, indicating the At a certain moment before, each historical speed The weight of .

[0011] Step 5: Deadlock determination is as follows: During the iteration of the algorithm, the system will dynamically update the weighted average speed based on the above formula. Subsequently, the robot will perform self-detection based on the updated weighted average speed value to determine whether its current state is a deadlock state, that is, whether it is in a dilemma of being unable to move forward effectively; In the In the iteration, if the following conditions are met: (41) Among them, the threshold is a minimum value that has been set; When the norm of the weighted average velocity is less than this minimum value, it means that several consecutive The value is below the threshold , it means that the robot's position has hardly changed over a period of time; In addition, it is also necessary to determine whether the robot is close to the target point. If the robot is not near the target point, that is, the robot's current position and the target point location The distance between them exceeds a certain fixed distance , combined with the weighted average speed, it means that it is very likely to be trapped by obstacles and fall into deadlock; As shown below: (42) in, is the tolerance distance for the robot to reach the target point; When the distance between the robot and the target point is less than When , the robot is considered to have reached the target point, navigation ends, and no deadlock judgment is made; When the robot is determined to be in a deadlock state, the deadlock escape strategy is activated to help the robot escape the deadlock area; otherwise, the robot is controlled in the direction of the fuzzy control plan with a speed of Move to .

[0012] Step 6: The deadlock escape strategy is as follows: Introducing the deadlock list ( ) record known deadlock areas, The robot will not re-enter any area in the deadlock zone, thus preventing the robot from repeatedly moving back and forth in a deadlock zone; Since the robot is in one time step At preset speed When moving, its normal moving distance is ,but ; Therefore, when the robot is determined to be deadlocked, the current position and the previous round position The midpoint between As the center, The radius The circular area Set up a deadlock region as follows: (43) (44) (45) For deadlock regions , it needs to be inserted into the quadtree, if the area If it intersects with the rectangular area of ​​a node, the area Stored in this node; First, calculate the center of the circle To rectangle boundary The nearest point , and then use the distance to determine whether the circle and rectangle intersect: (46) (47) When the result on the left side of formula (20) is less than or equal to the result on the right side, the two regions are considered to intersect, and then Stored in this node, indicating the area Access has been disabled to prevent the robot from repeatedly deadlocking in subsequent iterations; A robot in a deadlock zone will escape at the deadlock escape speed. Along the current location Deadlocked Regional Center The line between them moves outward from the area, and the movement speed is inversely proportional to the distance from the current position to the deadlock center, and is limited to range, ensuring smoother speed control: (48) (49) in, is the preset robot movement speed, is the original escape velocity without restriction; Considering that the robot may be in In the deadlock area, the inverse of the distance is used as the weight coefficient to merge the speed, as shown below: (50) (51) in, is the distance from the robot's current position to the center of each deadlock area, is the index of the deadlock region, is the center of the deadlock area that the robot is involved in, Deadlock Center The escape speed, It is a weight coefficient based on distance. The closer the distance, the greater the impact on the total speed. Starting from the next iteration, the robot will treat the areas in the deadlock list as invisible obstacles. The system calculates the distance between the robot's current position and the area based on the center and radius of each deadlock area, and uses this distance together with the distance of conventional obstacles as the basis for the fuzzy controller's distance input variable.

[0013] Step 7: Update the robot position The details are as follows: The algorithm needs to calculate the speed in the corresponding state according to the different states of the robot, and calculate the speed vector To calculate the robot's position at the next moment; When the robot is judged to be in a deadlock state, the robot cannot move normally. At this time, the robot's current speed will be adjusted to , indicating its adjustment speed when in deadlock state; If the robot is in a non-deadlock state, it can normally follow the speed set by the fuzzy control Move as shown below: (52) Ensures that the robot does not fall into a loop or collide in a deadlock state, but has the opportunity to adjust the speed through special to restore normalcy; According to the velocity vector , calculate the robot's position at the next time step , see the following formula: (53) in, is the robot's current position, is the time step; The robot calculates the target position at the next moment using the above formula and updates the calculation result as its new current position.

[0014] Step 8: Determine whether to terminate navigation. (1) Target point arrival judgment: When the robot's current position With the target point position The distance between them is less than or equal to the threshold When , the robot terminates navigation; (54) in, is the preset tolerance distance. If the above conditions are met, the system will determine that the robot has successfully reached the target point and immediately terminate the navigation process; (2) Maximum number of iterations: If the number of iterations of the algorithm reaches the preset maximum value , the system will automatically stop to prevent the navigation algorithm from falling into an infinite loop; When the navigation termination condition is reached, the system outputs the robot's complete navigation path , otherwise continue to return to step 2 for path planning.

[0015] (III) Beneficial effects Compared with the prior art, the present invention provides a hybrid path optimization method for a mobile robot in a complex dynamic environment, which has the following beneficial effects: 1. The present invention adopts a strategy based on a combination of quadtree data structure and fuzzy control algorithm. When the robot is detected to be trapped in a deadlock area, the mobile strategy can be adjusted in real time to avoid the robot from getting into trouble. This method can effectively identify and dynamically avoid the deadlock area, thereby improving the navigation efficiency of the robot in a complex environment.

[0016] 2. The present invention constructs a quadtree data structure to efficiently store and quickly query the detected deadlock areas. In each path planning process, the robot only needs to regard the known deadlock area as an obstacle and calculate the shortest distance to the area boundary for path planning, thereby avoiding falling into the same deadlock area and improving the overall navigation efficiency.

[0017] 3. The present invention designs a dynamic deadlock area setting mechanism, which can more accurately identify the area where the robot is trapped in a deadlock through a judgment formula based on a weighted average speed and a quadtree storage mechanism: the robot's motion state in a short period of time is detected through the weighted average speed formula, and whether it is trapped in a deadlock is determined in combination with a set threshold. Once a deadlock is detected, the deadlock area will be stored in the quadtree as a circular area, thereby preventing the robot from repeatedly entering the same deadlock area during subsequent navigation.

[0018] 4. The present invention enables the robot to navigate efficiently in unknown or complex environments by dynamically adjusting the deadlock area and efficient storage and query based on quadtrees. This not only significantly improves the adaptability of the system, but also reduces the dependence on environmental information, thereby enhancing the reliability and flexibility of the algorithm in practical applications.

[0019] 5. The present invention is not only suitable for indoor navigation and industrial automation environments, but can also be extended to unmanned driving, logistics robots, disaster rescue and other fields, providing a new solution for the efficient navigation of intelligent mobile robots in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is the algorithm flow chart of the present invention. DETAILED DESCRIPTION

[0021] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0022] Example like Figure 1 As shown, one embodiment of the present invention proposes: Known conditions: Maximum number of system iterations , time step , robot speed , speed threshold , distance threshold , the robot's historical iteration position set and speed set ; Explanation of known conditions and variables to be determined: first, Indicates the maximum number of iterations of the system. When the maximum number of iterations is reached, the system will stop executing the navigation algorithm to prevent the algorithm from falling into an infinite loop. is the time step, which is kept fixed in each iteration to ensure stability and accuracy during the calculation process. The robot will move at a constant speed in the navigation based on fuzzy control. Move in the calculated direction at a constant speed is a preset constant. Speed ​​threshold Used to determine whether the robot is in a deadlock state. Tolerance distance Used to determine whether the robot has reached the target point, that is, when the distance between the robot and the target point is less than When , the system will consider that the robot has successfully reached the target, thus terminating the execution of the navigation algorithm. and collection It is used to record all the position and speed information of the robot in historical iterations and initialize it to an empty set. Indicates The position at the iteration, Indicates The speed of the robot under iterations. It is a sequence of all coordinate points of the robot from the starting point to the target point.

[0023] A hybrid path optimization method for a mobile robot in a complex dynamic environment comprises the following steps: First, based on the fuzzy control algorithm, the system calculates the robot's steering angle and moving speed vector according to the target point direction and obstacle distance; Then, by weighted average speed Detect if the robot is stuck in an unknown deadlock area; If the robot is detected to be in deadlock, a circular area covering the current position is generated and stored in the deadlock area list of the quadtree structure. At the same time, the speed is adjusted according to the deadlock escape strategy. , so that the robot can quickly get out of trouble; If deadlock is not detected, the speed set by the fuzzy control algorithm is used. keep going; When the navigation termination condition is reached, the system outputs the navigation path , complete the navigation task.

[0024] Furthermore, the robot navigation path , the specific steps to obtain are as follows: Step 1: Construct a fuzzy controller as follows: (1) Determine the input and output variables: The fuzzy controller has a four-input and one-output structure; The input variables include: the angle between the target point direction and the robot's current direction , the unit is radian (rad), the range is ; A positive value indicates that the target is on the right side of the robot, and a negative value indicates that the target is on the left side; The distances to obstacles on the left, front, and right sides of the robot , , , the unit is meter (m), the range is ; The output variable is the steering angle that the robot should adjust , the unit is radian (rad), the range is ; Among them, positive values ​​indicate a right turn, and negative values ​​indicate a left turn; (2) Setting fuzzy sets: For the target direction , set its fuzzy set to ; Distance to obstacles , , , set its fuzzy set to ; For output variables , set its fuzzy set to ; (3) Setting the membership function: In the membership function setting of each variable, the generalized bell-shaped membership function is selected as the membership function of the input variable and the output variable; The reason is that the generalized bell-shaped membership function has a smooth membership change characteristic and can be used when the input value is close to the central value. Achieve smooth transition; In addition, by adjusting its parameters and , the shape and width of the membership function can be flexibly changed to adapt to different control requirements, thereby improving the response stability and flexibility of the system; The membership function is expressed as: (55) Among them, the parameters Control the width of the curve, parameters Controls the steepness of the curve, parameter Control the center position of the curve; In the input membership function, the target direction The parameters of the three membership functions They are [1.16, 3.28, -2.68; 0.89, 3.04, 0; 1.26, 3.28, 2.79] respectively; The parameters of the two membership functions for the three distance input variables are the same. They are [0.215, 4.77, 0.39; 0.211, 4.703, 0.899] respectively; In the output membership function, the steering angle The parameters of the five membership functions They are [0.76, 4.04, -3.07; 0.60, 4.05, -1.44; 0.66, 3.62, 0; 0.61, 4.09, 1.40; 0.82, 4.05, 3.14] respectively; (4) Designing fuzzy rules: By defining a fuzzy rule table, the robot can dynamically adjust its direction based on the target direction and obstacle distance, thereby avoiding obstacles and moving toward the target. A total of 24 rules are defined, and the rules are in the following format: (56) in, , , , The input variables are , , , In the The fuzzy set under the rules, is the fuzzy set of output variables, Iterates through all the rules The specific fuzzy control rules are shown in the following table: ; Here is an explanation of the first fuzzy rule: Rule 1: If the target direction is "leftward" and the left distance is "far" and the front distance is "far" and the right distance is "far", the turning angle is "left forward turn"; The fuzzy control rule generation logic is divided into two stages: first, the optimal initial direction is selected according to the distance to the obstacle, and the robot will preferentially move in the direction farthest from the obstacle to ensure safe obstacle avoidance; second, on this basis, the target point direction is adjusted so that the robot can move towards the target as much as possible while avoiding obstacles. The final steering angle is combined with the offset of the initial obstacle avoidance direction and the target point direction to achieve dynamic adjustment, ensuring that the robot can avoid obstacles efficiently and maintain goal orientation.

[0025] Step 2: Construct a deadlock list as follows: Introducing the deadlock list ( ) Record known deadlock areas. Its main purpose is to prevent the robot from getting stuck in a loop while moving toward the target point and to avoid it from repeatedly entering the detected deadlock area. By introducing a deadlock list , which can effectively improve the efficiency and reliability of navigation algorithms; Deadlock List The quadtree structure is used for storage and search. The quadtree can efficiently manage and retrieve spatial data, making it faster to make regional judgments in complex environments. Each node of the quadtree represents a rectangular area, which is defined as: (57) This rectangular area is defined by the lower left corner coordinates and the upper right corner coordinates OK, a quadtree is constructed by recursively dividing a rectangular region into four sub-regions (quadrants), each of which is itself a quadtree node; The principle of division is along Axis and The midpoints of the axes divide the current rectangular area into four sub-rectangles of equal size: (58) in: (59) The system first initializes an empty quadtree to store the boundaries of the circular deadlock area. At the same time, the root node boundary of the quadtree is set to the range of the entire workspace, thereby ensuring that the robot can dynamically update and query the deadlock area during navigation.

[0026] Step 3: Fuzzy control path planning is as follows: In fuzzy control path planning, the system uses the input fuzzy variables, the predefined membership function and the fuzzy rule table, and the Min-Max inference rule to obtain the output variables. Membership degree: (60) in, , , , The input variables are , , , In the The fuzzy set under the rules, is the output variable fuzzy set of ; is the membership function, Iterates through all the rules The formula first calculates the membership value of each input variable, and then The function gets the matching degree of each rule, and then uses The function takes the maximum value from all rules to determine the membership of the output variable; The output variable is obtained in fuzzy inference After the membership function is calculated, the fuzzy membership value needs to be converted into a specific numerical output to determine the steering angle that the robot should adjust. , the centroid method is used for defuzzification. Its principle is to take the centroid of the area enclosed by the fuzzy membership function curve and the horizontal coordinate as the final output value of fuzzy reasoning, that is: (61) in, is the possible value of the output variable, It is the membership degree under different values. The numerator represents the weighted product of each value and its membership degree, and the denominator is the sum of the membership degrees. The result is the final output value of fuzzy reasoning. , is the weighted average of all possible values; Based on the robot's current direction and the inferred steering angle , calculate the direction angle of the velocity vector for: (62) (63) in, is the current position, is the position at the last moment, and the two are used to calculate the current direction angle of the robot ; The robot follows the direction At speed When moving, its velocity vector on the two-dimensional plane is It is expressed as: (64) in, and Respectively indicate the speed and The weight in direction, is the preset uniform speed of the robot.

[0027] Step 4: Calculate the weighted average speed The details are as follows: To determine if the robot is trapped in an area, use the weighted average speed To determine whether the robot is trapped by an obstacle; The recursive formula of weighted velocity is shown as follows: (65) in, It is The weighted average speed of iterations, It is The weighted average speed of iterations, is the speed of the first iteration, It is The actual speed of the iterations, is the dynamic weight attenuation coefficient, which determines the influence of the current speed on the weighted average speed; The specific expressions are as follows: (66) in, Indicates the change between the current speed and the last weighted average speed, reflecting the amplitude of speed fluctuation. is the modulus of the current velocity, plus a small constant To prevent the denominator from being zero, and Function ensures Restricted to within the scope; When the speed changes greatly, The value increases, which means that the influence of the current actual speed on the weighted average speed increases, and it can respond to speed changes more quickly; when the speed is relatively stable, The value decreases, relying more on historical speed data, and obtaining a smoother weighted average speed; By recursive formula (11), we can It is expanded into the weighted sum of all historical velocities, as shown in the following equation (13): (67) in, is the current iteration number, is the index of a historical moment, indicating the At a certain moment before, each historical speed The weight of ; But because As a dynamically adjusted coefficient, the weight distribution will no longer be a simple fixed exponential decay, but will be adaptively adjusted according to the speed change. Therefore, the speed at the nearest moment will still have a greater impact, but the specific degree of impact will change with the speed change, which increases flexibility and adaptability; The weight distribution mechanism in the weighted average speed enables the model to strike a balance between smoothing historical data and responding to speed changes, and to respond more flexibly to drastic fluctuations in speed. This design helps the robot detect whether it is trapped by obstacles more accurately and promptly, because it emphasizes the importance of the current speed when there is a significant speed change, while retaining the stability of historical data when the change is small, thereby reducing the interference of historical data on current judgments and improving reaction sensitivity.

[0028] Step 5: Deadlock determination is as follows: During the iteration of the algorithm, the system will dynamically update the weighted average speed based on the above formula. Subsequently, the robot will perform self-detection based on the updated weighted average speed value to determine whether its current state is a deadlock state, that is, whether it is in a dilemma of being unable to move forward effectively; In the In the iteration, if the following conditions are met: (68) Among them, the threshold is a minimum value that has been set; When the norm of the weighted average velocity is less than this minimum value, it means that several consecutive The value is below the threshold , it means that the robot's position has hardly changed over a period of time; In addition, it is also necessary to determine whether the robot is close to the target point. If the robot is not near the target point, that is, the robot's current position and the target point location The distance between them exceeds a certain fixed distance , combined with the weighted average speed, it means that it is very likely to be trapped by obstacles and fall into deadlock; As shown below: (69) in, is the tolerance distance for the robot to reach the target point; When the distance between the robot and the target point is less than When , the robot is considered to have reached the target point, navigation ends, and no deadlock judgment is made; Therefore, we need to judge whether the robot is in a deadlock situation based on the weighted average speed when the robot has not reached the target point. If the robot is determined to be in a deadlock state, the deadlock escape strategy is activated to help the robot escape the deadlock area; otherwise, the robot is accelerated according to the speed in the direction of the fuzzy control planning. Move to .

[0029] Step 6: The deadlock escape strategy is as follows: Introducing the deadlock list ( ) record known deadlock areas, The robot will not re-enter any area in the deadlock zone, thus preventing the robot from repeatedly moving back and forth in a deadlock zone; Since the robot is in one time step At preset speed When moving, its normal moving distance is ,but ; Therefore, when the robot is determined to be deadlocked, the current position and the previous round position The midpoint between As the center, The radius The circular area Set up a deadlock region as follows: (70) (71) (72) For deadlock regions , it needs to be inserted into the quadtree, if the area If it intersects with the rectangular area of ​​a node, the area Stored in this node; First, calculate the center of the circle To rectangle boundary The nearest point , and then use the distance to determine whether the circle and rectangle intersect: (73) (74) When the result on the left side of formula (20) is less than or equal to the result on the right side, the two regions are considered to intersect, and then Stored in this node, indicating the area Access has been disabled to prevent the robot from repeatedly deadlocking in subsequent iterations; A robot in a deadlock zone will escape at the deadlock escape speed. Along the current location Deadlocked Regional Center The line between them moves outward from the area, and the movement speed is inversely proportional to the distance from the current position to the deadlock center, and is limited to range, ensuring smoother speed control: (75) (76) in, is the preset robot movement speed, is the original escape velocity without restriction; Considering that the robot may be in In the deadlock area, the inverse of the distance is used as the weight coefficient to merge the speed, as shown below: (77) (78) in, is the distance from the robot's current position to the center of each deadlock area, is the index of the deadlock region, is the center of the deadlock area that the robot is involved in, Deadlock Center The escape speed, It is a weight coefficient based on distance. The closer the distance, the greater the impact on the total speed. Starting from the next iteration, the robot will treat the areas in the deadlock list as invisible obstacles. The system calculates the distance between the robot's current position and the area based on the center and radius of each deadlock area, and uses this distance together with the distance of conventional obstacles as the basis for the fuzzy controller's distance input variable.

[0030] Step 7: Update the robot position The details are as follows: The algorithm needs to calculate the speed in the corresponding state according to the different states of the robot, and calculate the speed vector To calculate the robot's position at the next moment; When the robot is judged to be in a deadlock state, the robot cannot move normally. At this time, the robot's current speed will be adjusted to , indicating its adjustment speed when in deadlock state; If the robot is in a non-deadlock state, it can normally follow the speed set by the fuzzy control Move as shown below: (79) Ensures that the robot does not fall into a loop or collide in a deadlock state, but has the opportunity to adjust the speed through special to restore normalcy; According to the velocity vector , calculate the robot's position at the next time step , see the following formula: (80) in, is the robot's current position, is the time step; The robot calculates the target position at the next moment using the above formula and updates the calculation result as its new current position.

[0031] Step 8: Determine whether to terminate navigation. (1) Target point arrival judgment: When the robot's current position With the target point position The distance between them is less than or equal to the threshold When , the robot terminates navigation; (81) in, is the preset tolerance distance. If the above conditions are met, the system will determine that the robot has successfully reached the target point and immediately terminate the navigation process; (2) Maximum number of iterations: If the number of iterations of the algorithm reaches the preset maximum value , the system will automatically stop to prevent the navigation algorithm from falling into an infinite loop; When the navigation termination condition is reached, the system outputs the robot's complete navigation path , otherwise continue to return to step 2 for path planning.

[0032] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A hybrid path optimization method for a mobile robot in a complex dynamic environment, characterized in that: The steps include: First, based on the fuzzy control algorithm, the system calculates the robot's steering angle and moving speed vector according to the target point direction and obstacle distance; Then, by weighted average speed Detect if the robot is stuck in an unknown deadlock area; in; If the robot is detected to be in a deadlock, a circular area covering the current position is generated and stored in the deadlock area list of the quadtree structure. At the same time, the speed is adjusted according to the deadlock escape strategy. , so that the robot can quickly get out of trouble; If deadlock is not detected, the speed set by the fuzzy control algorithm is keep going; When the navigation termination condition is reached, the system outputs the navigation path , complete the navigation task.

2. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 1, characterized in that: The known conditions are pre-set as follows: Maximum number of iterations of the system , time step , robot speed , speed threshold , distance threshold , the robot's historical iteration position set and speed set ; Among them, the robot navigation path , the specific steps to obtain are as follows: Step 1: Construct fuzzy controller; Step 2: Build a deadlock list; Step 3: Fuzzy control path planning; Step 4: Calculate the weighted average speed ; Step 5: Deadlock determination; Step 6: Deadlock escape strategy; Step 7: Update the robot position ; Step 8: Determine whether to terminate navigation.

3. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The construction of the fuzzy controller is as follows: (1) Determine the input and output variables: The fuzzy controller has a four-input and one-output structure; The input variables include: the angle between the target point direction and the robot's current direction , the unit is radian (rad), the range is ; A positive value indicates that the target is on the right side of the robot, and a negative value indicates that the target is on the left side; The distances to obstacles on the left, front, and right sides of the robot , , , the unit is meter (m), the range is ; The output variable is the steering angle that the robot should adjust , the unit is radian (rad), the range is ; Among them, positive values ​​indicate a right turn, and negative values ​​indicate a left turn; (2) Setting fuzzy sets: For the target direction , set its fuzzy set to ; Distance to obstacles , , , set its fuzzy set to ; For output variables , set its fuzzy set to ; (3) Setting the membership function: In the membership function setting of each variable, the generalized bell-shaped membership function is selected as the membership function of the input variable and the output variable; The reason is that the generalized bell-shaped membership function has a smooth membership change characteristic and can be used when the input value is close to the central value. Achieve smooth transition; In addition, by adjusting its parameters and , the shape and width of the membership function can be flexibly changed to adapt to different control requirements, thereby improving the response stability and flexibility of the system; The membership function is expressed as: (1) Among them, the parameters Control the width of the curve, parameters Controls the steepness of the curve, parameter Control the center position of the curve; In the input membership function, the target direction The parameters of the three membership functions They are [1.16, 3.28, -2.68; 0.89, 3.04, 0; 1.26, 3.28, 2.79] respectively; The parameters of the two membership functions for the three distance input variables are the same. They are [0.215, 4.77, 0.39; 0.211, 4.703, 0.899] respectively; In the output membership function, the steering angle The parameters of the five membership functions They are [0.76, 4.04, -3.07; 0.60, 4.05, -1.44; 0.66, 3.62, 0; 0.61, 4.09, 1.40; 0.82, 4.05, 3.14] respectively; (4) Designing fuzzy rules: By defining a fuzzy rule table, the robot can dynamically adjust its direction based on the target direction and obstacle distance, thereby avoiding obstacles and moving toward the target. A total of 24 rules are defined, and the rules are in the following format: (2) in, , , , The input variables are , , , In the The fuzzy set under the rules, is the fuzzy set of output variables, Iterates through all the rules The specific fuzzy control rules are shown in the following table: ； Here is an explanation of the first fuzzy rule: Rule 1: If the target direction is "leftward" and the left distance is "far" and the front distance is "far" and the right distance is "far", the turning angle is "left forward turn"; The fuzzy control rule generation logic is divided into two stages: first, the optimal initial direction is selected according to the distance of the obstacle. The robot will move in the direction farthest from the obstacle first to ensure safe obstacle avoidance; second, on this basis, the target point direction is adjusted so that the robot can move towards the target as much as possible while avoiding obstacles. The final steering angle is dynamically adjusted by combining the initial obstacle avoidance direction with the offset of the target point direction to ensure that the robot can avoid obstacles efficiently and maintain goal orientation.

4. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The construction deadlock list is as follows: Introducing the deadlock list ( ) Record known deadlock areas. Its main purpose is to prevent the robot from getting stuck in a loop while moving toward the target point and to avoid it from repeatedly entering the detected deadlock area. By introducing a deadlock list , which can effectively improve the efficiency and reliability of navigation algorithms; Deadlock List The quadtree structure is used for storage and search. The quadtree can efficiently manage and retrieve spatial data, making it faster to make regional judgments in complex environments. Each node of the quadtree represents a rectangular area, which is defined as: (3) This rectangular area is defined by the lower left corner coordinates and the upper right corner coordinates OK, a quadtree is constructed by recursively dividing a rectangular region into four sub-regions (quadrants), each of which is itself a quadtree node; The principle of division is along Axis and The midpoints of the axes divide the current rectangular area into four sub-rectangles of equal size: (4) in: (5) The system first initializes an empty quadtree to store the boundaries of the circular deadlock area. At the same time, the root node boundary of the quadtree is set to the range of the entire workspace, thereby ensuring that the robot can dynamically update and query the deadlock area during navigation.

5. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The fuzzy control path planning is specifically as follows: In fuzzy control path planning, the system uses the input fuzzy variables, the predefined membership function and the fuzzy rule table, and the Min-Max inference rule to obtain the output variables. Membership degree: (6) in, , , , The input variables are , , , In the The fuzzy set under the rules, is the output variable fuzzy set of is the membership function, Iterates through all the rules The formula first calculates the membership value of each input variable, and then The function gets the matching degree of each rule, and then uses The function takes the maximum value from all rules to determine the membership of the output variable; The output variable is obtained in fuzzy inference After the membership function is calculated, the fuzzy membership value needs to be converted into a specific numerical output to determine the steering angle that the robot should adjust. , the centroid method is used for defuzzification. Its principle is to take the centroid of the area enclosed by the fuzzy membership function curve and the horizontal coordinate as the final output value of fuzzy reasoning, that is: (7) in, is the possible value of the output variable, It is the membership degree under different values. The numerator represents the weighted product of each value and its membership degree, and the denominator is the sum of the membership degrees. The result is the final output value of fuzzy reasoning. , is the weighted average of all possible values; Based on the robot's current direction and the inferred steering angle , calculate the direction angle of the velocity vector for: (8) (9) in, is the current position, is the position at the last moment, and the two are used to calculate the current direction angle of the robot ; The robot follows the direction At speed When moving, its velocity vector on the two-dimensional plane is It is expressed as: (10) in, and Respectively indicate the speed and The weight in direction, is the preset uniform speed of the robot.

6. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The weighted average speed is calculated The details are as follows: To determine if the robot is trapped in an area, use the weighted average speed To determine whether the robot is trapped by an obstacle; The recursive formula of weighted velocity is shown as follows: (11) in, It is The weighted average speed of iterations, It is The weighted average speed of iterations, is the speed of the first iteration, It is The actual speed of the iterations, is the dynamic weight attenuation coefficient, which determines the influence of the current speed on the weighted average speed; The specific expressions are as follows: (12) in, Indicates the change between the current speed and the last weighted average speed, reflecting the amplitude of speed fluctuation. is the modulus of the current velocity, plus a small constant To prevent the denominator from being zero, and Function ensures Restricted to within the scope; When the speed changes greatly, The value increases, which means that the influence of the current actual speed on the weighted average speed increases, and it can respond to speed changes more quickly; when the speed is relatively stable, The value decreases, relying more on historical speed data, and obtaining a smoother weighted average speed; By recursive formula (11), we can It is expanded into the weighted sum of all historical velocities, as shown in the following equation (13): (13) in, is the current iteration number, is the index of a historical moment, indicating the At a certain moment before, each historical speed The weight is .

7. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The deadlock determination is specifically as follows: During the iteration of the algorithm, the system will dynamically update the weighted average speed based on the above formula. Subsequently, the robot will perform self-detection based on the updated weighted average speed value to determine whether its current state is a deadlock state, that is, whether it is in a dilemma of being unable to move forward effectively; In the In the iteration, if the following conditions are met: (14) Among them, the threshold is a minimum value that has been set; When the norm of the weighted average velocity is less than this minimum value, it means that several consecutive The value is below the threshold , it means that the robot's position has hardly changed over a period of time; In addition, it is also necessary to determine whether the robot is close to the target point. If the robot is not near the target point, that is, the robot's current position and the target point location The distance between them exceeds a certain fixed distance , combined with the weighted average speed, it means that it is very likely to be trapped by obstacles and fall into deadlock; As shown below: (15) in, is the tolerance distance for the robot to reach the target point; When the distance between the robot and the target point is less than When , the robot is considered to have reached the target point, navigation ends, and no deadlock judgment is made; When the robot is determined to be in a deadlock state, the deadlock escape strategy is activated to help the robot escape the deadlock area; otherwise, the robot is controlled in the direction of the fuzzy control plan with a speed of Move to .

8. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The deadlock escape strategy is as follows: Introducing the deadlock list ( ) record known deadlock areas, The robot will not re-enter any area in the deadlock zone, thus preventing the robot from repeatedly moving back and forth in a deadlock zone; Since the robot is in one time step At preset speed When moving, its normal moving distance is ,but ; Therefore, when the robot is determined to be deadlocked, the current position and the previous round position The midpoint between As the center, The radius The circular area Set up a deadlock region as follows: (16) (17) (18) For deadlock regions , it needs to be inserted into the quadtree, if the area If it intersects with the rectangular area of ​​a node, the area Stored in the node; First, calculate the center of the circle To rectangle boundary The nearest point , and then use the distance to determine whether the circle and rectangle intersect: (19) (20) When the result on the left side of formula (20) is less than or equal to the result on the right side, the two regions are considered to intersect, and then Stored in this node, indicating the area Access has been disabled to prevent the robot from repeatedly deadlocking in subsequent iterations; A robot in a deadlock zone will escape at the deadlock escape speed. Along the current location Deadlocked Regional Center The line between them moves outward from the area, and the movement speed is inversely proportional to the distance from the current position to the deadlock center, and is limited to range, ensuring smoother speed control: (21) (22) in, is the preset robot movement speed, is the original escape velocity without restriction; Considering that the robot may be in In the deadlock area, the inverse of the distance is used as the weight coefficient to merge the speed, as shown below: (23) (24) in, is the distance from the robot's current position to the center of each deadlock area, is the index of the deadlock region, is the center of the deadlock area that the robot is involved in, Deadlock Center The escape speed, It is a weight coefficient based on distance. The closer the distance, the greater the impact on the total speed. Starting from the next iteration, the robot will treat the areas in the deadlock list as invisible obstacles. The system calculates the distance between the robot's current position and the area based on the center and radius of each deadlock area, and uses this distance together with the distance of conventional obstacles as the basis for the fuzzy controller's distance input variable.

9. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The update robot position The details are as follows: The algorithm needs to calculate the speed in the corresponding state according to the different states of the robot, and calculate the speed vector To calculate the robot's position at the next moment; When the robot is judged to be in a deadlock state, the robot cannot move normally. At this time, the robot's current speed will be adjusted to , indicating its adjustment speed when in deadlock state; If the robot is in a non-deadlock state, it can normally follow the speed set by the fuzzy control Move as shown below: (25) Ensures that the robot does not fall into a loop or collide in a deadlock state, but has the opportunity to adjust the speed through special to restore normalcy; According to the velocity vector , calculate the robot's position at the next time step , see the following formula: (26) in, is the robot's current position, is the time step; The robot calculates the target position at the next moment using the above formula and updates the calculation result as its new current position.

10. The hybrid path optimization method for a mobile robot in a complex dynamic environment according to claim 2, characterized in that: The determination of whether to terminate navigation is as follows: (1) Target point arrival judgment: When the robot's current position With the target point position The distance between them is less than or equal to the threshold When , the robot terminates navigation; (27) in, is the preset tolerance distance. If the above conditions are met, the system will determine that the robot has successfully reached the target point and immediately terminate the navigation process; (2) Maximum number of iterations: If the number of iterations of the algorithm reaches the preset maximum value , the system will automatically stop to prevent the navigation algorithm from falling into an infinite loop; When the navigation termination condition is reached, the system outputs the robot's complete navigation path , otherwise continue to return to step 2 for path planning.

Images