Robot adaptive navigation method and system

Abstract

The invention relates to a robot adaptive navigation method and system, and the method can enable a robot to autonomously collect environment information and improve self decisions in a task execution process, and solves the problem that navigation experience is difficult to effectively utilize. The obstacle position feature self-learning method based on the Gaussian mixture model is utilized, and the problem that environment prior information is difficult to obtain is solved. According to the obstacle probability real-time estimation method based on the exponential weighted moving average and the particle filter, the navigation efficiency of the robot in the uncertain environment of the aggregated crowd is improved. The self-adaptive navigation method for the robot is remarkable in effect and capable of effectively avoiding obstacles.

Description

Technical Field

[0001] The present invention relates to the technical field of robot navigation, and particularly to a method and system for robot adaptive navigation. Background Art

[0002] In the research of mobile robots, the research on global and local path planning algorithms, as the key to determining the motion strategy of robots, has always been a crucial part of robot autonomous navigation. However, in reality, the working environment where mobile robots are located is full of uncertainties. The execution of the navigation task of the robot may be affected by uncertain factors such as the entrance and exit being blocked by a dense crowd, opening and closing doors, etc. These uncertain factors pose challenges to the decision-making of the robot. Currently, most navigation strategies rely on global static maps and combine traditional local planning algorithms to deal with newly added obstacles, but this may lead to the robot frequently re-planning the path in a complex and uncertain environment. At the same time, traditional navigation systems usually adopt reactive immediate strategies to deal with sudden obstacles, and cannot remember and utilize the obstacle probability information in previous tasks, and it is also difficult to obtain the position information of obstacles for learning. Therefore, the navigation strategy cannot be optimized through historical navigation experience. For robots performing long-term tasks, this fixed strategy does not have environmental adaptability because the robot may frequently try to pass through routes with a high probability of being blocked. The Canadian Traveler Problem (CTP) is closely related to the motion planning of mobile robots in an uncertain environment. CTP involves finding the optimal path from the starting point to the ending point in a graph structure, and some edges in the graph may not be connected. Only when the robot reaches any end of the edge can it be confirmed whether the edge is passable. There are several problems with the current CTP solution algorithms: First, most CTP algorithms require the probability of the input edge being blocked as prior information, but this cannot be obtained in advance in the real environment; Second, existing CTP algorithms are mostly based on graph theory and are used more in discrete environments, but it is difficult to combine with the navigation system in a continuous real environment; In addition, most algorithms only focus on the strategy optimization of a single solution, fail to use past navigation experience to improve future strategies, and have poor real-time performance.

[0003] Many existing navigation algorithms (such as the fusion algorithm combining the A* algorithm and the DWA algorithm) perform poorly in highly uncertain environments, such as stations with dense crowds and high mobility or large shopping malls with many doors. Specifically, the existing navigation algorithms cannot take into account the uncertainty that causes changes in the traversability of the environment, so that the robot continues to adopt a fixed and rigid optimistic strategy, which makes it easy for the robot to be frequently blocked by obstacles during navigation, frequently replan the path, and have difficulty adapting to the changing environment. Secondly, the existing navigation algorithms ignore the location information of obstacles in the environment and cannot effectively use it for navigation tasks. In addition, the existing navigation algorithms do not use the robot's historical navigation experience, ignore the probability information of the obstacle appearance, and cause the robot to make unreasonable decisions. Finally, the existing navigation algorithms make the robot consider bypassing only when it is close to the crowd, and there is no escape mechanism, which easily leads to the failure of the robot being trapped by the crowd. Summary of the invention

[0004] To this end, the technical problem to be solved by the present invention is to overcome the problem that the navigation algorithm in the prior art performs poorly in a highly uncertain environment and has no escape mechanism, which easily leads to the robot being trapped by the crowd and the mission failing.

[0005] In order to solve the above technical problems, the present invention provides a robot adaptive navigation method, comprising:

[0006] Step S1: In an environment without dynamic obstacles, guide the robot to build an initial static map;

[0007] Step S2: input the starting point and the end point of the navigation task, and then obtain the topological feature points in the environment, which are key points used to characterize the robot's decision-making, and connect the topological feature points around each topological feature point in pairs with straight lines to form topological edges to form a topological graph;

[0008] Step S3: If the direct line between the two topological feature points does not pass through any fixed obstacle in the static map, the path cost is calculated using the Euclidean distance; if the direct line between the two topological feature points passes through a fixed obstacle in the static map, the Dijkstra algorithm is used to calculate a path cost that bypasses the fixed obstacle;

[0009] Step S4: after obtaining the topological feature points in the environment in step S2, taking the topological feature points as the center, setting the expansion parameters, and performing expansion to obtain a square area, and taking the square area as the dangerous area in the map;

[0010] Step S5: The robot performs navigation, reads the environmental probability information estimated in the previous navigation, and before the start of the current navigation task, uses the previous navigation experience to take the edge probability information of the probability topological map estimated previously as the prior information of the current navigation and inputs it to the improved A* algorithm in Step S7;

[0011] Step S6: Determine whether the robot reaches the end point or cannot execute the task. If it reaches or cannot execute the task, save and record the passing and blocking situations of the topological edges in the topological map during the current navigation task, and the current navigation task ends; if it does not reach, then execute Step S7;

[0012] Step S7: By improving the calculation method of the heuristic estimation cost h(n) of the A* algorithm, using the starting point, the end point, the topological feature points and topological edges of the topological map, the path cost, and the probability information of the edge blockage in the topological map as input parameters, finally output a coordinate sequence of a string of sub-goal points as the navigation path guiding the robot;

[0013] During the navigation process of the robot according to the navigation path, combine the dangerous area in Step S4 to determine whether the robot is blocked by an obstacle: if blocked by a door, the robot temporarily stops moving, updates its current position, marks the current topological edge as blocked and goes to Step S9 to update the edge blockage probability; if blocked by a crowd, let the robot evacuate the crowd position, and at the same time mark the topological edge where it is located as blocked, and go to Step S9 to update the edge blockage probability; if the robot is knocked down by the crowd and cannot execute the task, go to Step S6;

[0014] Step S9: When the robot successfully passes a topological edge or is blocked on a certain topological edge, based on the particle filter algorithm and the exponential weighted moving average method, and calculate the edge blockage probability of the topological edge passed at this time according to the type of obstacle;

[0015] Step S10: According to the coordinate sequence of a string of sub-goal points calculated in Step S7, guide the robot to move to the sub-goal point position, and at the same time execute Step S8 and Step S9;

[0016] Step S11: Loop Steps S5 - S10 until the multiple repeated navigation tasks end.

[0017] In an embodiment of the present invention, the method for obtaining topological feature points in the environment in Step S2 includes:

[0018] Step S21: Set a sequence of cruise points in the static map, and randomly shuffle the sequence of cruise points and randomly publish them as target points for the robot to execute tasks;

[0019] Step S22: The robot moves to the current target point according to the sequence of cruise points;

[0020] Step S23: At the current target point, determine whether the robot is blocked by a newly added obstacle: Take the average value of the lidar data within 30 degrees in front of the robot and compare it with the threshold. If it is less than the threshold, it is determined that the robot is blocked, and jump to Step S24; otherwise, continue to execute Step S22;

[0021] Step S24: If the robot is blocked, save and record the position where the robot is blocked at this time;

[0022] Step S25: The robot continuously cruises to collect a preset number of blocked positions as blocked feature points, and uses the Gaussian mixture model GMM to cluster the blocked feature points, and take the center point of the cluster as the topological feature point in the environment.

[0023] In an embodiment of the present invention, the method for improving the heuristic estimation cost of the A* algorithm in Step S7 includes:

[0024] Step S71: Set the number of samplings;

[0025] Step S72: Use the pre-estimated topological edge obstacle probability to randomly sample the topological edges in the topological graph, so as to generate a topological graph with the states of all edges determined;

[0026] Step S73: On the determined topological graph, use the Dijkstra algorithm to calculate the distance from the current node to the destination node;

[0027] Step S74: Cumulatively sum the obtained distances;

[0028] Step S75: Repeat Steps S72 to S74 until the set number of samplings is completed. Finally, take the average value of the cumulative distances as the value of the heuristic estimation cost h(n) of the improved A* algorithm, and take the value with the smallest value of the heuristic estimation cost h(n) each time as a sub-goal point, and finally obtain a coordinate sequence of a string of sub-goal points.

[0029] In an embodiment of the present invention, the method for determining whether the robot is blocked by an obstacle in Step S8 includes:

[0030] Step S81: Determine which obstacle the robot is blocked by: If a pedestrian is detected by the pedestrian detection algorithm, it is blocked by a person, otherwise it is blocked by a door;

[0031] Step S82: Determine that the robot is blocked by a door: If the average value of the lidar data 30 degrees in front of the robot is less than the threshold and its own position is in the dangerous area divided in Step S4, it is considered that the robot is blocked by a door;

[0032] Step S83: Determine whether the robot is blocked by a crowd: If it is detected by the pedestrian detection algorithm that the distance between a person and the robot is less than the threshold using laser, and at the same time the robot's own speed is less than a certain threshold at this time, it is considered that the robot is blocked by a crowd.

[0033] In an embodiment of the present invention, the calculation method of the topological edge blocking probability in step S9 includes:

[0034] Step S91: If the robot is blocked by a door, adopt a probability estimation strategy based on the particle filter algorithm PF, and execute step S92; if the robot is blocked by a crowd, jump to step S98;

[0035] Step S92: Particle initialization: First, sprinkle a number of particles on each edge, randomly assign values from 0 to 1 to represent the initial probability, and at the same time set the initial weights of each particle to be equal;

[0036] Step S93: Move and observe: Adopt a binomial likelihood model. When the robot observes edge blocking, particles with a higher estimated blocking probability are more likely to explain this observation result. Set the likelihood to the particle blocking probability estimate itself; otherwise, set the likelihood to 1 - probability estimate;

[0037] Step S94: Update the weights using the EWMA formula, and then normalize to obtain new weights;

[0038] Step S95: System resampling: First, calculate the cumulative weights: Add up the weights of all particles to form a cumulative weight array; Secondly, generate uniformly spaced sampling points: These points are evenly distributed in the [0, 1) interval; Then select particles: Use a search and sorting function to find the particle indices corresponding to each sampling point in the cumulative weight array, and select a new set of particles from the original particle array according to these indices. Particles with higher weights will be selected multiple times; Finally, update the weights and normalize;

[0039] Step S96: Calculate and estimate the probability: Take the weighted average of all particles to calculate the final blocking probability of each edge;

[0040] Step S97: Whenever the robot has a new observation result, repeat steps S93 - S95;

[0041] Step S98: Observe and record: Each time the robot attempts to pass through a specific edge, record the observation results, including blocked and unblocked.

[0042] In an embodiment of the present invention, in step S94, the weights are updated using the EWMA formula, and then

[0043] normalized to obtain new weights. The update formula is:

[0044] W t =W t-1*[λL+(1-λ)W t-1

[0045] where L is the likelihood, λ is the smoothing factor used to control the memory length of the model for historical data, and W t-1 represents the weight before update.

[0046] In an embodiment of the present invention, when using the sub-goal point as the current navigation goal of the robot for navigation in step S10, the A* algorithm is used as the global path planning algorithm, and the dynamic window approach (DWA) is used as the local path planning algorithm. The combination of the two guides the robot towards the sub-goal point.

[0047] To solve the above technical problems, the present invention provides a robot adaptive navigation system, including:

[0048] The first construction module: guiding the robot to construct an initial static map in an environment without dynamic obstacles;

[0049] The second construction module: inputting the starting point and the ending point of the navigation task, and then obtaining the topological feature points in the environment. The topological feature points are key points used to represent the decisions made by the robot. The topological feature points around each topological feature point are connected pairwise by straight lines to form topological edges, so as to form a topological map;

[0050] The cost calculation module: if the direct connection between two topological feature points does not pass through any fixed obstacles in the static map, the Euclidean distance is used to calculate the cost; if the direct connection between two topological feature points crosses the fixed obstacles in the static map, the Dijkstra algorithm is used to calculate the cost of a path that bypasses the fixed obstacles;

[0051] The dangerous area construction module: after obtaining the topological feature points in the environment in the second construction module, taking the topological feature points as the center, setting the inflation parameter, and performing inflation to obtain a square area, and taking the square area as the dangerous area in the map;

[0052] The navigation module: the robot performs navigation, reads the environmental probability information estimated in the previous navigation. Before the start of the current navigation task, using the previous navigation experience, taking the edge probability information of the probability topological map estimated previously as the prior information of the current navigation, and inputting it to the decision-making and solving algorithm;

[0053] The first judgment module: judging whether the robot reaches the end point or cannot execute the task. If it reaches or cannot execute the task, save and record the passing and blocking situations of the topological edges in the topological map during the current navigation task, and the current navigation task ends; if it does not reach, then execute the path planning module;

[0054] ​Path planning module: By improving the calculation method of the heuristic estimated cost h(n) of the A* algorithm, using the starting point, the end point, the topological feature points and topological edges of the topological map, the path cost, and the probability information of edge obstacles in the topological map as input parameters, and finally outputting a coordinate sequence of a string of sub-goal points as the navigation path to guide the robot;

[0055] Second judgment module: During the navigation process of the robot according to the navigation path, in combination with the dangerous area in the dangerous area construction module, it judges whether the robot is blocked by obstacles: If blocked by a door, the robot temporarily stops moving, updates its current position, marks the current topological edge as an obstacle and turns to the edge obstacle probability calculation module to update the edge obstacle probability; If blocked by a crowd, the robot evacuates from the crowd position, marks the topological edge where it is located as an obstacle at the same time, and turns to the edge obstacle probability calculation module to update the edge obstacle probability; If the robot is knocked down by the crowd and cannot perform the task, it turns to the navigation module;

[0056] Edge obstacle probability calculation module: When the robot successfully passes through a topological edge or is blocked on a certain topological edge, based on the particle filter algorithm and the exponentially weighted moving average method, and calculates the edge obstacle probability of the topological edge passed at this time according to the type of obstacle;

[0057] Movement module: Taking the sub-goal point as the current navigation target of the robot, guiding the robot to move to the position of the sub-goal point according to the coordinate sequence of a string of sub-goal points calculated in the path planning module, and at the same time executing the second judgment module and the edge obstacle probability calculation module;

[0058] Loop module: The process of looping from the navigation module to the movement module until the end of multiple repeated navigation tasks.

[0059] To solve the above technical problems, the present invention provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, the steps of the above robot adaptive navigation method are implemented.

[0060] To solve the above technical problems, the present invention provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above robot adaptive navigation method are implemented.

[0061] The above technical solutions of the present invention have the following advantages compared with the prior art:

[0062] The robot adaptive navigation method constructed by the present invention enables the robot to autonomously collect environmental information and improve its own decision-making during the task execution process, and solves the problem of difficult to effectively utilize navigation experience;

[0063] The present invention designs a self - learning method for obstacle position features based on the Gaussian Mixture Model (GMM), which solves the problem of difficult acquisition of environmental prior information;

[0064] The present invention designs a real - time estimation method for obstacle probability based on Exponentially Weighted Moving Average (EWMA) and particle filtering, which improves the navigation efficiency of the robot in an uncertain environment such as a crowded population. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] In order to make the content of the present invention easier to be clearly understood, the following further details the present invention according to specific embodiments of the present invention in conjunction with the accompanying drawings.

[0066] Figure 1 is the flowchart of the method of the present invention;

[0067] Figure 2 is a schematic diagram of the construction of the probability topological map of the present invention;

[0068] Figure 3 is a schematic diagram of obtaining topological feature points by Gaussian mixture model clustering of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0069] The following further illustrates the present invention in conjunction with the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and be able to implement it, but the embodiments cited do not limit the present invention.

[0070] Embodiment 1

[0071] Referring to Figure 1 as shown, the present invention relates to a robot adaptive navigation method, which specifically includes the following steps:

[0072] S1. Construct an initial map. First, set the starting position of the robot in an unexplored environment without dynamic obstacles. The robot relies on its Simultaneous Localization and Mapping (SLAM) technology, adopts the Gmapping mapping algorithm, and combines the sensors it carries to collect the surrounding environmental information. During this process, the robot also uses the Adaptive Monte Carlo Localization (AMCL) technology to accurately determine its position and ensure the accuracy of map construction. The robot can generate an initial two - dimensional grid map, which will serve as the basic map for performing tasks and can be used to guide the robot to complete the static map construction of the entire environment through remote control;

[0073] S2. Construct a probability topological map. First, input the starting point and the ending point of the navigation task. Secondly, obtain the topological feature points in the environment, which are used to represent the key points for the robot to make decisions, generally on both sides of the door or around the areas where new obstacles frequently appear. Then, connect the nearby feature points of all feature points in pairs to form topological edges;

[0074] S3. Initialize the probabilistic topological map. Cost initialization: If the direct connection between two topological feature points does not pass through any fixed obstacles in the static map, then the path cost is calculated directly using the Euclidean distance. If the connection crosses a fixed obstacle, the Dijkstra algorithm is used to calculate the path cost to bypass the obstacle. Probability initialization: Before a series of navigation tasks start, assume that all edges are equally likely to be blocked, and set the blocking probability of all topological edges to 0.5; for details, please refer to Figure 2 ;

[0075] S4. Set the dangerous area. After obtaining the topological feature points in the environment in step S2, use these points as the center and set the inflation parameter to obtain a square area as the dangerous area in the map. This represents the area where obstacles often appear in the map.

[0076] S5. Read the environmental probability information estimated in the previous navigation. Before the start of this navigation task, utilize the previous navigation experience and input the edge probability information of the previously estimated probabilistic topological map as the prior information for this navigation to the improved A* algorithm in step S7;

[0077] S6. Determine whether the robot has reached the end point or cannot execute the task. If it has reached or cannot execute the task, save and record the passing and blocking situations of the topological map edges in this navigation task, and this navigation task ends. If it has not reached, then execute S7;

[0078] S7. Solve the navigation strategy in an uncertain environment. By modifying the calculation method of the heuristic estimated cost h(n) of the A* algorithm, the algorithm uses the starting point, end point, nodes and edges of the topological map, edge costs, and the probability information of edge blocking in the map as input parameters. Finally, a sequence of point coordinates is output to guide the navigation path of the robot;

[0079] S8. Determine whether the robot is completely blocked by an obstacle and which obstacle it is blocked by. (1) If blocked by a door, immediately temporarily stop moving, update its current position, mark the current topological edge as blocked, and go to execute step S9; (2) If blocked by a crowd, let the robot get out of trouble first, evacuate from the crowd position, and at the same time mark the current edge as blocked, and go to S9 to calculate and update the edge probability. If the robot is knocked down by the crowd and cannot execute the task, then go to S6;

[0080] S9. Estimate the topological edge blocking probability. When the robot successfully passes a topological edge or is blocked on a certain edge, based on the particle filter algorithm (PF) and the exponentially weighted moving average method (EWMA), and calculate the topological edge blocking probability information at this time according to the type of obstacle;

[0081] S10. Use the sub-goal points as the current navigation goals of the robot. Guide the robot chassis to move towards the sub-goal positions according to the coordinate sequence of a series of sub-goal points calculated in S7. At this time, execute S8 and S9 simultaneously;

[0082] S11. Loop through steps S5 - S10 until a series of repeated navigation tasks end.

[0083] In the above technical solution, obtaining the topological feature points in the environment in step S2 includes the following steps:

[0084] S21. Cruise task setting. Set a sequence of cruise points in the global static map and randomly shuffle this sequence and publish it to the robot as the target points to execute tasks. The purpose of random shuffling is to make the recorded feature points more evenly distributed in the map, facilitating subsequent clustering.

[0085] S22. The robot moves to the target points according to the sequence until enough feature points are recorded when being blocked.

[0086] S23. Record blocked feature points. Determine whether the robot is blocked by newly added obstacles. Compare the mean value of the lidar data within 30 degrees in front of the robot with the threshold. If it is less than the threshold, it is determined to be blocked and jump to S24; otherwise, continue to execute S22.

[0087] S24. If blocked, save and record the position where the robot is blocked at this time and visually publish it to Rviz.

[0088] S25. Gaussian Mixture Model (GMM) clustering. After the robot cruises for a period of time and collects enough feature points of blocked positions, use GMM to cluster these points and take the center point of the cluster as the topological feature points in the environment. For details, please refer to Figure 3 .

[0089] In the above technical solution, the improved A* algorithm heuristic estimation cost in step S7 includes the following steps:

[0090] S71. Set the number of samplings. More samplings help the robot tend to choose a more reliable path rather than just a shorter path, but this will increase the computational complexity;

[0091] S72. Use the pre-estimated topological edge blockage probability to randomly sample the topological edges in the topological graph, thereby generating a topological graph with the states of all edges determined;

[0092] S73. On this fixed topological graph, use the Dijkstra algorithm to calculate the distance from the current node to the destination node;

[0093] S74. Cumulatively sum the obtained distances;

[0094] S75. Repeat steps S72 to S74 until the set number of samplings is completed. Finally, take the average value of the cumulative distances as the value of the heuristic estimated cost h(n) of the improved A* algorithm, and take the value with the minimum value of the heuristic estimated cost h(n) each time as a sub-goal point. Finally, obtain a coordinate sequence of a string of sub-goal points.

[0095] In the above technical solution, the determination of whether the path is completely blocked in step S8 includes the following steps:

[0096] S81. Determine which obstacle the robot is blocked by. If the laser detection human leg algorithm leg_detector detects a pedestrian, it is blocked by a person; otherwise, it is blocked by a door.

[0097] S82. Determine that the robot is blocked by a door. If the average value of the lidar data in front of the robot at 30 degrees is less than the threshold, and the robot's own position is in the dangerous area divided in step S4, it is considered that the robot is blocked by a door.

[0098] S83. Determine that the robot is blocked by a crowd. If the laser detection human leg algorithm leg_detector detects that the distance between a person and the robot is less than the threshold, and the robot's own speed is less than a certain threshold at this time, it is considered that the robot is blocked by a crowd.

[0099] In the above technical solution, the topological edge obstacle probability estimation in step S9 includes the following steps:

[0100] S91. If the robot is blocked by a door, adopt a probability estimation strategy based on the particle filter algorithm (PF) and execute step S92. If it is blocked by a crowd, jump to S98.

[0101] S92. Particle initialization. First, sprinkle some particles on each edge, randomly assign values from 0 to 1 to represent the initial probability, and at the same time set the initial weight of each particle to be equal.

[0102] S93. Move and observe. Adopt a binomial likelihood model. When the robot observes an edge obstacle, particles with a higher estimated blocking probability are more likely to explain this observation result. Set the likelihood to the particle obstacle probability estimate itself. Conversely, set the likelihood to 1 - probability estimate.

[0103] S94. Weight update. Use the EWMA formula to update the weights, and then normalize to obtain new weights. The update formula is:

[0104] W t =W t-1 *[λL+ ( 1-λ)W t-1 ​

[0105] Among them, L is the likelihood, and λ is the smoothing factor used to control the memory length of the model for historical data. W t-1 represents the weight before update. The advantage of EWMA is that it can adjust parameters to attach importance to recent observed data. Different values of λ have an impact on probability estimation. A smaller λ responds faster to new changes. In this embodiment, it is desired that the robot has a faster response speed for probability estimation of obstacles such as humans to ensure safety.

[0106] S95. System resampling. First, calculate the cumulative weight: Add up the weights of all particles to form a cumulative weight array. Second, generate uniformly spaced sampling points: These points are evenly distributed in the interval [0, 1). Then, select particles: Use a search and sorting function to find the particle indices corresponding to each sampling point in the cumulative weight array, and select a new set of particles from the original particle array according to these indices. Particles with high weights will be selected multiple times. Finally, update the weights and normalize them.

[0107] S96. Calculate and estimate the probability. Take the weighted average of all particles to calculate the final obstacle probability of each edge.

[0108] S97. Whenever the robot has a new observation result. Repeat steps S93 - S95.

[0109] S98. Observation and recording: Each time the robot attempts to pass through a specific edge, record the observation results, including blocked and unblocked.

[0110] In the above technical solution, in step S10, when navigating to the sub - target point, the A* algorithm is used as the global path planning algorithm, and the Dynamic Window Approach (DWA) is used as the local path planning algorithm. The combination of the two guides the robot to the sub - target point while realizing basic dynamic obstacle avoidance.

[0111] Experimental comparison

[0112] To fully prove the effectiveness of the present invention, the present invention has conducted comparative experiments in a simulation scenario with the fusion algorithm that combines the A* algorithm and the DWA algorithm globally (hereinafter referred to as the "fusion algorithm") in an environment with 4 types of population distributions. This algorithm aims to verify the superiority of the method of this embodiment for the autonomous decision-making of robots in an uncertain environment with clustered populations and its adaptability to environmental changes. For each environment, each algorithm will conduct 20 experiments. After 20 experiments in one environment are completed, the population will move to form a new population distribution, and then 20 more experiments will be conducted, and so on until the experiments in all four environments are tested. Note: After switching the environment, the probability estimation of the environment by the method of this embodiment will not be re-initialized, aiming to test the adjustment ability of the algorithm and its adaptability to the new environment - the new environment may cause large deviations in the probability parameters of the previously constructed probability topology map, which need to be adjusted based on the current observations.

[0113] For the experimental environment, a total of 160 experiments were conducted. Among them, 80 experiments were conducted using the fusion algorithm, and 80 experiments were conducted using the method of this embodiment. The experimental comparison metrics are the time (s) used to complete the navigation task, the completion rate of this task, the time (s) of being too close to pedestrians during the task, and the total length (m) of the path. Note: The total time and path length are only recorded when the task is successfully completed, and are not recorded when the task fails (the robot collides with a pedestrian and cannot continue the task). In addition, being too close to a person is set as the robot being less than 1 m away from the person. Similarly, this embodiment is only interested in the time of being too close to a person during this task when the task is successful.

[0114] The experimental data shows (Table 1 corresponds to the experimental data of rounds 1 - 20, Table 2 corresponds to the experimental data of rounds 21 - 40, Table 3 corresponds to the experimental data of rounds 41 - 60, Table 4 corresponds to the experimental data of rounds 61 - 80, and Table 5 corresponds to the comprehensive data of 80 experiments) that in the 20 - round experiment in simulation environment 1 (as shown in Table 1), compared with the fusion algorithm, the method of this embodiment has an average task completion rate increased by 34%, the average time of being too close to a person decreased by 49.71 s, and the average navigation time decreased by 49 s.

[0115] In the 20 - round experiment in simulation environment 2 (as shown in Table 2), compared with the fusion algorithm, the method of this embodiment has an average task completion rate increased by 40%, the average time of being too close to a person decreased by 25.91 s, and the average navigation time decreased by 5 s.

[0116] In the 20 - round experiment in simulation environment 3 (as shown in Table 3), compared with the fusion algorithm, the method of this embodiment has an average task completion rate increased by 21%, the average time of being too close to a person decreased by 17.35 s, and the average navigation time decreased by 3 s.

[0117] In the 20 rounds of experiments in simulation environment 4 (as shown in Table 3), compared with the fusion algorithm, the average task completion rate of the method in this embodiment increased by 34%, and the average time of being too close to people decreased by 41.85 seconds.

[0118] In the comprehensive data of 80 experiments in Table 5, compared with the fusion algorithm, the average task success rate of the method in this embodiment increased by 34.9%, the average time of being too close to people decreased by 33.7 seconds, and the average navigation time decreased by 12 seconds.

[0119] It is worth discussing that the average navigation time of the method in this embodiment in environment 4 is 10 seconds slower than that of the fusion algorithm. This is because our method selects a safer path to bypass pedestrians, with a longer path length, sacrificing the path length to give priority to task success rate and stability. The fusion algorithm adopts a greedy strategy, resulting in a shorter path and a shorter time when successful, but with a very low success rate, and a very long time of being too close to pedestrians for the successfully navigated path, with a high risk level and invading the private space of pedestrians.

[0120] Experiments show that the method in this embodiment has a better task execution success rate than traditional algorithms in complex and uncertain environments, can adapt to uncertain environments, and can gradually adjust strategies to adapt to changing uncertain environments, reflecting the adaptive characteristics of the method in this embodiment.

[0121] Table 1

[0122]

[0123]

[0124] Table 2

[0125]

[0126]

[0127] Table 3

[0128]

[0129]

[0130] Table 4

[0131]

[0132] Table 5

[0133] Algorithm Navigation success rate Average time too close to people (s) Average navigation time (s) Average path length (m) Fusion algorithm 28.75％ 34.32 171 45.43 This embodiment 63.65％ 0.62 159 53.33

[0134] Embodiment 2

[0135] This embodiment provides a robot adaptive navigation system, including:

[0136] The first construction module: In an environment without dynamic obstacles, guide the robot to construct an initial static map;

[0137] The second construction module: Input the starting point and the ending point of the navigation task, and then obtain the topological feature points in the environment. The topological feature points are key points used to represent the robot's decision-making. Connect the topological feature points around each topological feature point in pairs by straight lines to form topological edges, so as to form a topological map;

[0138] The cost calculation module: If the direct connection between two topological feature points does not pass through any fixed obstacles in the static map, use the Euclidean distance to calculate the path cost; if the direct connection between two topological feature points crosses the fixed obstacles in the static map, use the Dijkstra algorithm to calculate a path cost that bypasses the fixed obstacles;

[0139] The dangerous area construction module: After obtaining the topological feature points in the environment in the second construction module, take the topological feature points as the center, set the inflation parameter, and perform inflation to obtain a square area, and use the square area as the dangerous area in the map;

[0140] The navigation module: The robot performs navigation, reads the environmental probability information estimated in the previous navigation. Before the start of the current navigation task, use the previous navigation experience, and use the edge probability information of the probability topological map estimated previously as the prior information of the current navigation, and input it to the improved A* algorithm;

[0141] The first judgment module: Judge whether the robot reaches the end point or cannot execute the task. If it reaches or cannot execute the task, save and record the passing and blocking situations of the topological edges in the topological map during the current navigation task, and the current navigation task ends; if it does not reach, execute the path planning module;

[0142] The path planning module: By improving the calculation method of the heuristic estimation cost h(n) of the A* algorithm, use the starting point, the ending point, the topological feature points and topological edges of the topological map, the path cost, and the probability information of the edge blockage in the topological map as input parameters, and finally output a coordinate sequence of a series of sub-goal points as the navigation path to guide the robot;

[0143] Second Judgment Module: During the navigation of the robot along the navigation path, in combination with the dangerous area in the Dangerous Area Construction Module, it determines whether the robot is blocked by an obstacle: If blocked by a door, the robot temporarily stops moving, updates its current position, marks the current topological edge as blocked, and transfers to the Edge Blocking Probability Calculation Module to update the edge blocking probability; If blocked by a crowd, the robot evacuates from the crowd position, marks the topological edge where it is located as blocked, and transfers to the Edge Blocking Probability Calculation Module to update the edge blocking probability; If the robot is knocked down by the crowd and unable to perform tasks, it transfers to the Navigation Module;

[0144] Edge Blocking Probability Calculation Module: When the robot successfully passes through a topological edge or is blocked on a certain topological edge, based on the particle filter algorithm and the exponentially weighted moving average method, and according to the type of obstacle, it calculates the edge blocking probability of the topological edge passed at this time;

[0145] Movement Module: According to the coordinate sequence of a series of sub-goal points calculated in the Path Planning Module, it guides the robot to move towards the sub-goal point position, and simultaneously executes the Second Judgment Module and the Edge Blocking Probability Calculation Module;

[0146] Loop Module: The process of looping from the Navigation Module to the Movement Module until the end of multiple repeated navigation tasks.

[0147] Embodiment III

[0148] This embodiment provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the robot adaptive navigation method described in Embodiment I.

[0149] Embodiment IV

[0150] This embodiment provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it implements the steps of the robot adaptive navigation method described in Embodiment I.

[0151] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of the present application can be implemented in various computer languages, for example, object-oriented programming languages such as Java and interpreted scripting languages such as JavaScript.

[0152] This application is described with reference to the flowcharts and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowchart and / or block diagram can be implemented by computer program instructions, as well as the combination of flows and / or blocks in the flowchart and / or block diagram. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in one or more of the flows Figure 1 one or more of the flows and / or blocks Figure 1 or one or more of the blocks.

[0153] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means that implement the functions specified in one or more of the flows Figure 1 one or more of the flows and / or blocks Figure 1 or one or more of the blocks.

[0154] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operational steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more of the flows Figure 1 one or more of the flows and / or blocks Figure 1 or one or more of the blocks.

[0155] Although the preferred embodiments of the present application have been described, those skilled in the art can make additional changes and modifications once they learn the basic creative concepts. Therefore, the appended claims are intended to be construed to include the preferred embodiments as well as all changes and modifications falling within the scope of the present application.

[0156] Obviously, the above embodiments are merely examples for clear illustration and are not limitations on the implementation manners. For those of ordinary skill in the art, other different forms of changes or variations can be made based on the above description. It is not necessary and impossible to enumerate all the implementation manners here. And the obvious changes or variations derived therefrom are still within the protection scope of the present invention.

Claims

1. A robot adaptive navigation method, characterized in that: include: Step S1: In an environment without dynamic obstacles, guide the robot to build an initial static map; Step S2: input the starting point and the end point of the navigation task, and then obtain the topological feature points in the environment, which are key points used to characterize the robot's decision-making, and connect the topological feature points around each topological feature point in pairs with straight lines to form topological edges to form a topological graph; Step S3: If the direct line between the two topological feature points does not pass through any fixed obstacle in the static map, the path cost is calculated using the Euclidean distance; if the direct line between the two topological feature points passes through a fixed obstacle in the static map, the Dijkstra algorithm is used to calculate a path cost that bypasses the fixed obstacle; Step S4: after obtaining the topological feature points in the environment in step S2, taking the topological feature points as the center, setting the expansion parameters, and performing expansion to obtain a square area, and taking the square area as the dangerous area in the map; Step S5: The robot performs navigation and reads the environmental probability information estimated in the last navigation. Before starting this navigation task, the robot uses the previous navigation experience to take the edge probability information of the previously estimated probability topology map as the prior information for this navigation and inputs it into the improved A* algorithm in step S7. Step S6: Determine whether the robot has reached the end point or cannot perform the task. If it has reached the end point or cannot perform the task, save and record the passage and obstruction of the topological edge in the topological graph in this navigation task, and this navigation task ends; if it has not reached the end point, execute step S7; Step S7: by improving the calculation method of the heuristic estimation cost h(n) of the A* algorithm, the starting point, the end point, the topological feature points and topological edges of the topological graph, the path cost and the probability information of the edge obstruction in the topological graph are used as input parameters, and finally a sequence of coordinates of a string of sub-target points is output as the navigation path to guide the robot; Step S8: When the robot is navigating according to the navigation path, it is determined whether the robot is blocked by obstacles in combination with the dangerous area in step S4: if it is blocked by a door, the robot temporarily stops moving, updates its current position, marks the current topological edge as an obstacle, and goes to step S9 to update the edge obstruction probability; if it is blocked by a crowd, the robot is allowed to evacuate the crowd position, and marks the topological edge as an obstacle, and goes to step S9 to update the edge obstruction probability; if the robot is knocked down by the crowd and cannot perform the task, go to step S6; Step S9: When the robot successfully passes through a topological edge or is blocked on a topological edge, the edge blocking probability of the topological edge passed at this time is calculated based on the particle filter algorithm and the exponential weighted moving average method and according to the type of obstacle; Step S10: according to the coordinate sequence of a series of sub-target points calculated in step S7, guide the robot to move to the position of the sub-target point, and execute steps S8 and S9 at the same time; Step S11: looping steps S5 to S10 until the navigation task is completed after multiple repetitions.

2. The robot adaptive navigation method according to claim 1, characterized in that: The method for obtaining topological feature points in the environment in step S2 includes: Step S21: setting a cruise point sequence in a static map, and randomly disrupting the cruise point sequence and randomly publishing it as a target point to the robot to perform the task; Step S22: the robot moves to the current target point according to the cruise point sequence; Step S23: At the current target point, determine whether the robot is blocked by a newly added obstacle: compare the average value of the laser radar data within a range of 30 degrees in front of the robot with the threshold. If it is less than the threshold, it is determined that the robot is blocked and jump to step S24; otherwise, continue to execute step S22; Step S24: if the robot is obstructed, then the obstructed position of the robot is saved and recorded; Step S25: The robot collects a preset number of obstacle positions as obstacle feature points by continuously cruising, clusters the obstacle feature points using a Gaussian mixture model GMM, and takes the center point of the cluster as a topological feature point in the environment.

3. The robot adaptive navigation method according to claim 1, characterized in that: The method for improving the A* algorithm heuristic cost estimation in step S7 includes: Step S71: setting the sampling times; Step S72: using the pre-estimated topological edge blocking probability, randomly sampling the topological edges in the topological graph, thereby generating a topological graph in which the states of all edges are determined; Step S73: On the determined topology graph, use the Dijkstra algorithm to calculate the distance from the current node to the destination node; Step S74: accumulating and summing the obtained distances; Step S75: Repeat steps S72 to S74 until the set number of sampling times is completed, and finally take the average value of the accumulated distance as the value of the heuristic estimation cost h(n) of the improved A* algorithm, and take the minimum value of each heuristic estimation cost h(n) as a sub-target point, and finally obtain a coordinate sequence of a series of sub-target points.

4. The robot adaptive navigation method according to claim 1, characterized in that: The method for determining whether the robot is blocked by an obstacle in step S8 includes: Step S81: Determine which obstacle the robot is blocked by: if a pedestrian is detected by the pedestrian detection algorithm, then the robot is blocked by a person; otherwise, the robot is blocked by a door; Step S82: Determine whether the robot is blocked by a door: If the average value of the laser radar data at 30 degrees in front of the robot is less than the threshold, and the robot's position is in the dangerous area divided in step S4, the robot is considered to be blocked by a door; Step S83: Determine whether the robot is obstructed by the crowd: If the pedestrian detection algorithm uses laser to detect that the distance between the person and the robot is less than a threshold, and the robot's own speed is less than a certain threshold at this time, the robot is considered to be obstructed by the crowd.

5. The robot adaptive navigation method according to claim 1, characterized in that: The method for calculating the topological edge obstruction probability in step S9 includes: Step S91: If the robot is blocked by a door, a probability estimation strategy based on a particle filter algorithm PF is adopted, and step S92 is executed; if the robot is blocked by a crowd, jump to step S98; Step S92: Particle initialization: First, sprinkle a number of particles on each edge, randomly assign values ​​0-1 to represent the initial probability, and set the initial weight of each particle to be equal; Step S93: Move and observe: Using the binomial likelihood model, when the robot observes edge obstruction, it is estimated that particles with a high probability of obstruction are more able to explain this observation result, and the likelihood is set to the particle obstruction probability estimate itself; otherwise, the likelihood is set to 1-probability estimate; Step S94: Update the weight using the EWMA formula and then normalize to obtain a new weight. The update formula is: W t =W t-1 *[λL+(1-λ)W t-1 ] Among them, L is the likelihood, λ is the smoothing factor used to control the model's memory length for historical data, and W t-1 represents the weight before updating; Step S95: System resampling: First, calculate the cumulative weight: add up the weights of all particles to form a cumulative weight array; second, generate evenly spaced sampling points: these points are evenly distributed in the interval [0,1); then select particles: use the search sorting function to find the particle index corresponding to each sampling point in the cumulative weight array, and select a new set of particles from the original particle array based on these indexes. Particles with high weights will be selected multiple times; finally, update the weights and normalize them; Step S96: Calculate and estimate the probability: take the weighted average of all particles to calculate the final obstruction probability of each edge; Step S97: Whenever the robot has new observation results, repeat steps S93-S95; Step S98: Observation and recording: Each time the robot attempts to pass a specific edge, the observation results are recorded, including obstruction and non-obstruction.

6. The robot adaptive navigation method according to claim 5, characterized in that: In step S94, the weight is updated using the EWMA formula, and then normalized to obtain a new weight. The update formula is: W t =W t-1 *[λL+(1-λ)W t-1 ] Among them, L is the likelihood, λ is the smoothing factor used to control the model's memory length for historical data, and W t-1 Represents the weight before updating.

7. The robot adaptive navigation method according to claim 1, characterized in that: In step S10, when the sub-target point is used as the current navigation target of the robot for navigation, the A* algorithm is used as the global path planning algorithm, and the dynamic window method DWA is used as the local path planning algorithm, and the combination of the two guides the robot to the sub-target point.

8. A robot adaptive navigation system, characterized in that: include: The first construction module: guides the robot to build an initial static map in an environment without dynamic obstacles; The second construction module: input the starting point and end point of the navigation task, and then obtain the topological feature points in the environment. The topological feature points are key points used to characterize the robot's decision-making. The topological feature points around each topological feature point are connected in pairs with straight lines to form topological edges to form a topological graph. Cost calculation module: If the direct line between two topological feature points does not pass through any fixed obstacles in the static map, the Euclidean distance is used to calculate the path cost; if the direct line between two topological feature points passes through a fixed obstacle in the static map, the Dijkstra algorithm is used to calculate a path cost that bypasses the fixed obstacle; Dangerous area construction module: After obtaining the topological feature points in the environment in the second construction module, the expansion parameters are set with the topological feature points as the center, and the square area is expanded to be obtained, and the square area is used as the dangerous area in the map; Navigation module: The robot performs navigation and reads the environmental probability information estimated in the last navigation. Before starting this navigation task, it uses the previous navigation experience to take the edge probability information of the previously estimated probability topology map as the prior information for this navigation and inputs it into the improved A* algorithm. The first judgment module: judges whether the robot has reached the end point or cannot perform the task. If it has reached or cannot perform the task, save and record the passage and obstruction of the topological edge in the topological graph in this navigation task, and this navigation task ends; If it has not arrived, the path planning module is executed; Path planning module: By improving the calculation method of the heuristic estimation cost h(n) of the A* algorithm, the starting point, the end point, the topological feature points and topological edges of the topological graph, the path cost, and the probability information of edge obstruction in the topological graph are used as input parameters, and finally a sequence of coordinates of sub-target points is output as the navigation path to guide the robot; Second judgment module: When the robot is navigating according to the navigation path, it is combined with the dangerous area in the dangerous area construction module to determine whether the robot is blocked by obstacles: if it is blocked by a door, the robot temporarily stops moving, updates its current position, marks the current topological edge as an obstacle and goes to the edge obstruction probability calculation module to update the edge obstruction probability; if it is blocked by a crowd, the robot is evacuated from the crowd, and the topological edge is marked as an obstacle, and the robot goes to the edge obstruction probability calculation module to update the edge obstruction probability; if the robot is knocked down by the crowd and cannot perform the task, it goes to the navigation module; Edge obstruction probability calculation module: When the robot successfully passes through a topological edge or is blocked on a topological edge, the edge obstruction probability of the topological edge passed at this time is calculated based on the particle filter algorithm and the exponentially weighted moving average method and the type of obstacle; Movement module: guides the robot to move to the sub-target point according to the coordinate sequence of the sub-target points calculated in the path planning module, and executes the second judgment module and the edge obstruction probability calculation module at the same time; Loop module: loop the process from navigation module to movement module until the repeated navigation tasks are completed.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the robot adaptive navigation method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the robot adaptive navigation method according to any one of claims 1 to 7 are implemented.

Images