CN113219975B

CN113219975B - Route optimization method, route planning method, chip and robot

Info

Publication number: CN113219975B
Application number: CN202110502487.3A
Authority: CN
Inventors: 孙永强; 李永勇
Original assignee: Zhuhai Amicro Semiconductor Co Ltd
Current assignee: Zhuhai Amicro Semiconductor Co Ltd
Priority date: 2021-05-08
Filing date: 2021-05-08
Publication date: 2024-04-05
Anticipated expiration: 2041-05-08
Also published as: CN113219975A

Abstract

The invention discloses a route optimization method, a route planning method, a chip and a robot, wherein the robot sequentially uses free grid points generated randomly and starting and stopping points of the local route as search centers on the basis of searching the local route in a narrow area, searches passable free grid points in a constructed map area under different screening conditions, increases the sampling number of the free grid points distributed in a difficult area, and enables the planned feasible route to cover all the free grid areas of the corresponding map area as much as possible by combining the local route, so that the free grid points are interconnected to form a more complete route network.

Description

Route optimization method, route planning method, chip and robot

Technical Field

The invention relates to the technical field of robot path planning, in particular to a path optimization method based on a grid map, a path planning method, a chip and a robot.

Background

Path planning refers to searching a collision-free path from a starting point to an end point according to a certain evaluation standard, such as shortest path or least planning time, in a certain environment.

The random path map method (Probabilistic Roadmaps, PRM) is a relatively common path map method, has the great advantage that the complexity of the random path map method mainly depends on the difficulty of searching paths, is basically irrelevant to the complexity of the whole planning environment and the dimension of the planning space, has strong actual operability, and is suitable for engineering application.

Compared with the traditional graph searching algorithm, the random path graph method (Probabilistic Roadmaps, PRM) has the advantages that the searching speed is higher, and the random path graph method can be reused; however, when there are many obstacles or too few sampling points and the distribution is not reasonable, the PRM algorithm may fail in path planning, so the PRM algorithm is not probabilistic. Specifically, in a planning space, the planning space comprises a difficult area, a normal area and an obstacle area, wherein the difficult area can be divided into an open area and a crack between obstacles, namely a narrow passage; because the general PRM method is to uniformly generate free nodes with equal probability, the number of random free nodes generated in an open area is easy to be insufficient, and the route map is incomplete; and the distribution of free nodes in a narrow area is not ideal, so that some feasible paths are omitted, and the planned route is often incomplete and the coverage rate in the corresponding area is not high.

In summary, in the process of path planning by adopting a general PRM algorithm in the prior art, because the robot has a slip condition, a sensor for positioning has accumulated errors or visual map optimization also generates errors, the robot easily marks the narrow areas as areas occupied by obstacles on a constructed grid map, and the narrow areas are not really blocked in an actual motion scene, so that a feasible path passing through the difficult area can not be accurately and completely searched, and because the number of free nodes in the narrow areas in a planning space is incompletely distributed, certain real passable paths are easily missed to cause navigation escape failure.

Disclosure of Invention

In order to solve the technical problems, the technical scheme of the invention discloses a route optimization method, a route planning method, a chip and a robot, wherein on the basis of moving and searching a local route in a narrow area, the robot sequentially uses free grid points generated randomly and starting and stopping points of the local route as search centers, searches passable free grid points in a constructed map area based on different screening conditions, increases the sampling number of the free grid points distributed in a difficult area, and enables the planned feasible route to cover all the free grid areas of the corresponding map area as much as possible in combination with the local route, thereby forming a more complete route network. The specific technical scheme is as follows:

A route optimization method, comprising: firstly, controlling a robot to move along a preset path and planning at least one local route; the preset path is a preset path for supporting the robot to pass through a narrow area; then, randomly searching a first preset number of grid points meeting first communication screening conditions in the grid map constructed by the robot; respectively taking the grid points meeting the first communication screening condition, the starting points of all local routes and the end points of the local routes as a first search center, searching out grid points meeting the second communication screening condition, connecting the first search center and all the searched grid points meeting the second communication screening condition into a first feasible route, and further determining that the first feasible route is connected with the local routes; when grid points meeting the first communication screening condition exist in all the child nodes of the starting points of all the local routes and all the child nodes of the ending points of all the local routes, determining that a first feasible route is connected with the local routes to form a complete candidate navigation network; when the grid points meeting the first communication screening condition do not exist in all the child nodes of the starting points of all the local routes and all the child nodes of the ending points of all the local routes, the starting points of all the local routes and the ending points of all the local routes are used as second search centers to search out grid points meeting the third communication screening condition, the second search centers and all the searched grid points meeting the third communication screening condition are connected to form a second feasible route, and then the first feasible route and the second feasible route are determined to be connected with the local routes to form a complete candidate navigation network.

Compared with the prior art, the technical scheme utilizes the local route formed by traversing the robot in the narrow area and the grid points which are randomly searched in the grid map representing the relatively open area and provided with the first communication screening condition by the robot to expand the grid points meeting the second communication screening condition, and the grid points are connected with the current searching center to form a first feasible route so as to improve the distribution density of free grid points used for entering and exiting the difficult area; and then on the basis of the connected first feasible route, continuously determining whether to continuously expand grid points meeting the third communication screening condition with lower requirements on connectivity by utilizing the two ends of the local route according to the distribution condition of the grid points of the first communication screening condition in the adjacent areas of the two ends of the local route, and further interconnecting the grid points with the current search center to form a second feasible route, so as to further interconnect the grid points to form a more complete candidate navigation network.

Therefore, no matter facing open map areas or narrow areas with complex obstacle distribution, the more grid points are connected to two ends of the local route, the more complete the candidate navigation network is formed by the feasible routes formed by the local route and the expansion interconnection, the higher the coverage rate of the passable area is, the more favorable the robot searches for a path effectively passing through the narrow area, the probability of missing the feasible route passing through the difficult area is reduced, and the planning accuracy of the navigation path passing through the narrow area is accelerated.

Further, the method for randomly searching the first preset number of grid points meeting the first communication screening condition in the grid map constructed by the robot specifically includes: calculating a product based on the ratio of the size of the grid map constructed by the robot to the side length of the single grid, obtaining the total number of all grids distributed in the grid map constructed by the robot, and determining the total number as the first preset number; wherein, the grid points are represented by grid center points corresponding to one grid; randomly searching grid points meeting the first communication screening condition in the grid map by taking the current position of the robot as a searching center, and adding a pre-established feasible route set until randomly searching and obtaining the first preset number of grid points meeting the first communication screening condition; wherein, set up the grid point that satisfies the first communication screening condition as the first communication node. According to the technical scheme, the total number of grids distributed in the grid map constructed by the robot is used for representing the distribution density of the nodes in the grid map, and the characteristics of the grid map, namely the discrete map, are met; and then, the first communication nodes for representing the total number of the grids distributed in the grid map are randomly searched, so that the distribution density of the first communication nodes in the grid map area is improved, the minimum value of the number of free grid points in the open area is ensured, the generated candidate navigation network is optimized, and the completeness of the candidate navigation network is improved.

Further, the first communication screening condition is: and the free grid point of the current random search does not exist in the feasible route node set, and no barrier grid point exists in the area with the current random search free grid point as the circle center, the search radius being one machine body diameter and no unknown grid point exists in the area, so that the free grid point of the current random search is the first communication node. The explored free grid points are in the exploreable areas near the first communication nodes, so that the first communication nodes which are randomly searched in the constructed grid map have more traffic significance.

Further, the route optimization method further includes: setting the starting point and the ending point of the local route as second communication nodes; meanwhile, on the local route, setting an intermediate node between a starting point and an ending point of the local route as a third communication node; recording the position information of the child node of the second communication node in the predicted passing node set, and adding the second communication node into the feasible route node set; recording the position information of the child node of the third communication node in the same predicted passing node set, and adding the third communication node into the feasible route node set; so that the feasible route node set stores one complete path node which is used for representing a corresponding unique local route in the prediction passing node set; the predicted passing node set is created in the process that the robot moves along the preset path; the second communication node and the child nodes thereof are adjacent and communicated; the third connected node and its child nodes are adjacent and connected. And the path nodes of the local route planned by the robot are transferred from the predicted passing node set to the feasible route node set one by one, so that the original local route is restored in the feasible route node set in a connecting mode based on the recorded position information of the child nodes.

Further, the route optimization method further includes: when the second communication node and the third communication node join the feasible route node set, the second communication node and the third communication node in the feasible route node set are sequentially connected into the local route according to the adjacent relation of the child node and the father node thereof based on the recorded position information of the child node. And based on the position information of the child node, expanding a new node according to a preset step length to expand the third communication node, then continuously expanding a subsequent third communication node by taking the third communication node which is currently expanded as a new starting point until the third communication node is expanded to the new second communication node, and sequentially connecting the complete local routes according to the sequence of sequential expansion.

Further, the method for searching the grid points meeting the second communication screening condition by using the grid points meeting the first communication screening condition, the starting points of all local routes and the end points thereof as the first search center, and connecting the first search center and all the searched grid points meeting the second communication screening condition into a first feasible route specifically comprises the following steps: the first communication node and the second communication node are respectively used as the first search center, grid points meeting second communication screening conditions are searched in the feasible route node sets, the grid points meeting the second communication screening conditions are set as first legal sub-nodes of the corresponding first search centers and added into the sub-node sets of the corresponding first search centers, wherein the sub-node sets of the corresponding first search centers also exist in the feasible route node sets, and each first communication node and each second communication node have a corresponding sub-node set in the feasible route node sets; and then connecting the same first search center and all first legal sub-nodes to form the first feasible route, and further determining that the first feasible route is connected with the local route.

According to the technical scheme, grid points meeting the second communication screening condition are selected in the surrounding areas of the start and stop points of the local route and the surrounding areas of the first communication nodes, the neighborhood of each free grid point is not required to be focused like a traditional PRM algorithm, child nodes are added in the neighborhood, and the searching efficiency is low; on the other hand, compared with the method that the method is limited to searching the grid points meeting the second communication screening condition in the narrow area (the area represented by the area around the start-stop point of the local route), the method can screen the grid points meeting the second communication screening condition in a wider area and record the grid points as legal child nodes for connecting the local route, and has the advantages that a plurality of routes for connecting the narrow area, the open area, the normal area and the obstacle area are generated through fusion connection of the local route and the first feasible route, so that the connectivity of the candidate navigation network in the narrow area and the open area is improved.

Further, the second communication screening condition includes: the grid points which do not have barrier grid points on the connecting line with the first search center and the connecting line length with the first search center is smaller than a preset distance threshold value are the first communication nodes or the second communication nodes; the preset distance threshold is preset according to specific environmental conditions; the preset distance threshold is greater than the side length of the single grid. In the discrete grid area corresponding to the feasible route node set, free grid points exist in the corresponding distance range of the first search center and are enough to be connected into a route, and the situation that the number of the free grid points in the area is not too small to generate first legal child nodes meeting the requirements is avoided.

Further, when the grid points meeting the first communication screening condition exist in all the sub-nodes of the starting points of all the local routes and all the sub-nodes of the ending points of all the local routes, the method for determining that the first feasible route is connected with the local routes to form a complete candidate navigation network comprises the following steps: if the fact that all the sub-nodes of the second communication nodes in the feasible route node set are all provided with the first communication nodes is detected, determining that two ends of a local route to which all the second communication nodes belong are respectively connected with all the first communication nodes in the sub-node set added into the corresponding first search center, and further determining that the local route and the first feasible route are connected to form a complete candidate navigation network; the currently detected child nodes of the second communication node are in the child node set corresponding to the same second communication node.

Compared with the prior art, the first communication nodes which are randomly searched in the grid map corresponding to the open area and the planned local routes in the narrow area form a complete candidate navigation network after the reasonable number of free grid points and the communication environment are provided, so that the defect that the generation completeness of the road map is poor due to the fact that the free nodes are uniformly distributed in the probability of being completely equal in a general PRM algorithm is overcome, and feasible routes passing through the narrow area and the open area are not easy to miss.

Further, when the grid points meeting the first communication screening condition do not exist in the child nodes of the starting points of all the local routes and the child nodes of the ending points of all the local routes, the grid points meeting the third communication screening condition are searched out by taking the starting points of all the local routes and the ending points of all the local routes as second search centers, and then the second search centers and all the searched grid points meeting the third communication screening condition are connected into a second feasible route, the method comprises the following steps: when detecting that all the sub-nodes of the second communication nodes in the feasible route node set do not have the first communication nodes, respectively taking each second communication node in the feasible route node set as the second search center, searching out grid points meeting third communication screening conditions in the feasible route node set, setting the grid points meeting the third communication screening conditions as third legal sub-nodes of the corresponding second search center, adding the third legal sub-nodes into the sub-node set of the corresponding second search center, increasing elements of the sub-node set corresponding to each second communication node, connecting the same second search center with all the third legal sub-nodes as the second feasible route, and further determining that the local route is connected with the second feasible route; the currently detected child nodes of the second communication node are in the child node set corresponding to the same second communication node. Compared with the method for searching the grid points meeting the second communication screening condition, the method for searching the grid points in the adjacent space of the second communication node reduces the searching range, increases elements of the sub-node set corresponding to the second communication node, and relatively improves the distribution probability of the free grid points in the adjacent space of the second searching center. The probability of missing a viable path through the difficult region is reduced.

Further, the third communication screening condition includes: among all grid points which have no obstacle grid points and no unknown grid points on the connection line with the second search center, the grid point with the shortest connection line length with the second search center is the first connection node. In the map in the area with insufficient free grid points, passable path nodes with shortest distance are searched out by increasing the acquisition point quantity near the start and stop points of the local route, and the connectivity standard between the nodes is reduced, so that the path search of an algorithm in a narrow channel is enhanced, and the algorithm is more suitable for complex terrains. And compared with the search area matched with the second communication screening condition, the search area matched with the third communication screening condition is reduced, so that the speed of route planning is improved.

Further, in the complete candidate navigation network, two ends of the local route are respectively used for connecting one end point of the first feasible route and/or one end point of the second feasible route, but neither the first feasible route nor the second feasible route is connected with an intermediate node of the local route; so as to search a route from a preset navigation starting point to a preset navigation ending point in the complete candidate navigation network by using a path search algorithm; the narrow area is a crack channel of two or more obstacles, the crack channel is a gap corresponding to the narrowest part between the two obstacles, and the width of the gap is larger than or equal to the diameter of the robot body; wherein the node set is a data structure occupying memory space. According to the technical scheme, the random free grid points generated by the steps are connected with the two ends of the local route to form a graph, and the graph is the generated candidate navigation network capable of covering the narrow area and the open area and is used as a relatively complete search source of the candidate navigation route in the narrow area by a path search algorithm, so that an optimal navigation path which meets requirements better is found, and the probability of missing a feasible path passing through the difficult area is reduced.

Further, the method for planning the local route comprises the following steps: step S1, controlling the robot to move along a preset path until judging that the preset search area meets a first preset round-area passing condition, and then entering step S2; s2, recording the current position of the robot as a first path node, simultaneously creating a new predicted passing node set, adding the first path node into the predicted passing node set, and then entering step S3; step S3, recording the current position of the robot as a second path node every time the robot moves a straight line distance of a body diameter along the preset path, and then entering step S4; step S4, when judging that the second path node meets a second preset round domain passing condition, adding the second path node into the predicted passing node set in the step S2; the path nodes stored in the predicted passing node sets are connected into a corresponding local route according to the sequence of joining, so that one predicted passing node set correspondingly represents a unique local route.

Compared with the prior art, the technical scheme is suitable for searching the local route for passing through the narrow area under the normal working state of the robot, a first preset round-domain passing condition is set as a pre-judging condition of the narrow area, and a path node source of the local route corresponding to the predicted passing node set is provided; and then the robot is enabled to continuously move along the original path, a second preset circle passing condition is set as a precise judging condition of the narrow area, and the grid point sources of the local routes corresponding to the predicted passing node set are continuously provided, so that the robot can be qualified for judging the narrow area after moving a certain distance, the local routes which are collected by the predicted passing node set and are used for being connected into grid points are more complete, the map grid error environment under the narrow area is more suitable, and the practically passable route is provided for the robot without paying attention to the marking information of the grids on the route one by one.

Further, when the second path node is judged not to meet the second preset circle domain passing condition, the method further comprises the following steps: step S6, according to the number of path nodes stored in the predicted passing node set, storing the predicted passing node set into a local route node set, and returning to the step S1; wherein, one local route node set has at least one predicted passing node set; within the same local route node set, the head element and the tail element of any one of the prediction passing node sets are unique, and the head element and the tail element of the same prediction passing node set are not identical. On the basis of the technical scheme, if the corresponding preset round-domain passing condition is not met, stopping searching for the predicted passing node set, and determining that the path nodes in a single predicted passing node set can be connected into an independent local route. And in a normal moving state of the robot, searching out a route point set capable of overcoming map drift errors by iteratively executing the related steps. The success rate of the robot for finding the effective navigation path in the scene with more complex obstacle space layout is increased.

Further, the method for storing the predicted passing node set into the same local route node set according to the number of path nodes stored in the predicted passing node set, and then returning to step S1 specifically includes: when the number of path nodes stored in the predicted passing node set is smaller than 2, deleting the predicted passing node set, and returning to the step S1 to create a new predicted passing node set; when the number of path nodes stored in the predicted passing node set is greater than or equal to 2, the predicted passing node set is used for independently representing a local route and is stored in the local route node set so as to be called by a path searching algorithm, and then the step S1 is returned to create a new predicted passing node set; the path node corresponding to the first element in each predicted passing node set is the starting point of a corresponding local route, and the path node corresponding to the tail element in each predicted passing node set is the ending point of a corresponding local route. The path nodes in each predicted passing node set are matched and connected to form a local route, so that a local route which can be used for predicting the robot to pass without barriers in a corresponding area is formed; in the technical scheme, if the number of path nodes finally acquired by a prediction passing node set is as small as one line is difficult to connect, invalid path nodes are reduced; and finally storing the path nodes obtained by the prediction passing node set as a whole to the local route node set for storing a set of local routes, so that the access structure of the local routes is reasonable and orderly.

Further, the first preset round-field traffic condition includes: the ratio of the area of the first non-passable area to the area of the pre-search area is greater than the first traffic evaluation value; the first non-passable area is a grid area consisting of unknown grid points and barrier grid points in the grid area corresponding to the first circular area; the first traffic evaluation value is a pre-judgment threshold value set for overcoming a marking error of a free grid point existing in the construction grid map; the pre-search area is a first circular area taking the current position of the robot as a circle center and the diameter of the robot body as a radius. The method can be used for preliminarily judging whether the robot starts to enter the narrow area, belongs to a rough judgment condition, and further judgment is carried out in the continuous moving process of the robot. However, whether the robot is allowed to pass through the grid areas corresponding to the first circular areas is not considered one by one, so that the influence of marking errors of the grids is reduced.

Further, the second preset round-domain traffic condition includes: in a second circular area taking the current position of the robot as the center of a circle and the diameter of the body of the robot as the radius, the ratio of the area of the second non-passable area to the area of the second circular area is larger than a second passable evaluation value; the second non-passable area is a grid area consisting of unknown grid points and barrier grid points in the grid area corresponding to the second circular area; the second pass evaluation value is a judgment threshold value set for overcoming a marking error of a free grid point existing in the construction grid map, and is larger than the first pass evaluation value. So as to improve the judgment accuracy. Compared with the judgment of the first preset circular domain passing condition, the technical scheme improves the judgment precision of a narrow area and reduces the influence of the marking information of a single grid point by judging the trafficability with higher precision through judging the area proportion occupied by the non-trafficable area in the second circular area after the robot moves to the diameter of the robot body which is one distance away from the newly recorded path node.

Further, the second preset round-domain traffic condition includes: searching a path of a second path node which is recorded latest to a first path node which is recorded latest in a second circular area which takes the first path node which is recorded latest as a circle center and takes the diameter of a machine body which is a preset multiple as a radius by using a path searching algorithm; wherein, the fuselage diameter of predetermineeing the multiple sets up to: the second circular areas are not allowed to intersect other marked grid areas. So as to avoid the participation of the planned paths in other marked grid areas in judgment, and further avoid the occurrence of erroneous judgment.

The technical scheme is reasonable in size setting of the search area, namely the second circular area, and the situation that the robot is guided to other areas and is not guided to pass through the current narrow area due to the fact that the robot intersects other known map areas to add the planned route in the relevant area into the search area is avoided; on the other hand, whether a complete navigation path from a starting point to an end point can be planned in the second circular area currently explored is judged, so that the passable area of the second circular area is not influenced by the marking position of the barrier grid; thereby improving the judgment accuracy of the narrow region.

Further, in the process of executing the step S1, the method further includes: firstly judging whether the robot moves to the end point of the preset path, if so, stopping executing the planning method of the local route, otherwise, controlling the robot to continuously move along the preset path, and judging whether the pre-search area meets a first preset circle domain passing condition; in the process of executing step S3, further comprising: judging whether the robot moves to the end point of the preset path, if so, stopping executing the planning method of the local route, otherwise, controlling the robot to continue to move along the preset path. And stopping the robot from continuously moving and executing the planning method of the local route in time so as to meet the intelligent planning requirement and avoid the situation that the robot performs route searching without limit, thereby consuming excessive unnecessary computing resources.

A path planning method, the path planning method comprising: executing the route optimization method; after the route optimization method is performed, configuring the first feasible route, the second feasible route and the local route as candidate navigation routes in a candidate navigation network; further comprises: step 1, setting a navigation starting point and a navigation end point in a grid map; step 2, setting a navigation starting point as a current father node and as a node to be traversed; step 3, performing neighborhood search in the grid map by taking the current father node as a search center, wherein 8 grid points adjacent to the current father node are respectively used as child nodes of the current father node; step 4, when the child node searched in the neighborhood in the step 3 is one end point of a corresponding candidate navigation route in the candidate navigation network searched in advance, setting the other end point of the corresponding candidate navigation route as a node to be traversed; setting all intermediate nodes and the end points between two end points of a corresponding candidate navigation route as traversed nodes, and setting free grid points which are not searched in the step 3 and are not on the corresponding candidate navigation route as nodes to be traversed; then enter step 5; wherein, all nodes set as to be traversed are correspondingly recorded with the position information of the father node for subsequent backtracking of the path node; step 5, selecting a node with the minimum path cost and value from all nodes to be traversed as a next father node, judging whether the next father node is the navigation destination or not, if so, starting from the navigation destination based on the recorded position information of the father node, and sequentially connecting child nodes and father nodes except all intermediate nodes of the candidate navigation route and corresponding one end points in step 4 until the nodes are connected to the navigation start point, and planning a path from the navigation start point to the navigation destination; otherwise, the next father node is updated to the current father node in the step 3, and the step 3 is returned.

Compared with the prior art, the path planning scheme sequentially selects a node with the minimum path cost and value from all nodes to be traversed as a father node to conduct neighborhood search from the navigation starting points, free grid points which are obtained by neighborhood search and are not located on candidate navigation routes and one endpoint (starting point or end point) with identification significance on the candidate navigation routes are added into the caching space of the nodes to be traversed, the corresponding father node is recorded to be used as path node backtracking information, meanwhile interference caused by repeated search of intermediate nodes (although the intermediate nodes are all navigation target points suitable for the narrow areas) on the candidate navigation routes is eliminated until the father node is the navigation end point, the candidate navigation routes which are searched through connection are reversely connected to the navigation starting points according to the position relation of the father node and the father node, so that a navigation route candidate meeting the passing condition is searched through a mature path searching algorithm to obtain a navigation route, the narrow area mapped to the narrow mark error of the narrow area is easy to be generated, the robot can effectively enter and exit along the whole navigation route, the effective planning path is reduced, and the probability of the narrow area is failed in planning and the planning of the robot is reduced.

Further, the step 4 further comprises setting the free grid points searched in the neighborhood in the step 3 as nodes to be traversed when the child nodes searched in the neighborhood are not endpoints of all candidate navigation routes, recording the position information of the father nodes of the free grid points searched in the neighborhood in the step 3, and then entering the step 5. Therefore, the searching range of the child node is enlarged, the route starting point in the candidate navigation network can be searched in a despread mode in a larger range, and the planned navigation path has better practicability and representativeness.

Further, in the step 4, when the child node searched in the neighborhood of the step 3 is the start point of a corresponding one of the candidate navigation routes in the candidate navigation network, setting the end point of the corresponding one of the candidate navigation routes as a node to be traversed, and recording the position information of the parent node of the end point of the corresponding one of the candidate navigation routes, so that each of the candidate navigation routes uses the end point thereof as route identification information in the candidate navigation network; setting all intermediate nodes between two endpoints of a corresponding candidate navigation route and the starting point of the same candidate navigation route as traversed nodes; and the parent node of each node on the corresponding one of the candidate navigation routes is positioned at a node position adjacent to the node along the path extending direction of the starting point of the corresponding one of the candidate navigation routes to the ending point of the corresponding one of the candidate navigation routes.

Further, in the step 4, when the child node searched in the neighborhood of the step 3 is the end point of the corresponding one of the candidate navigation routes in the candidate navigation network, setting the start point of the corresponding one of the candidate navigation routes as a node to be traversed, and recording the position information of the parent node of the start point of the corresponding one of the candidate navigation routes, so that each of the candidate navigation routes uses the start point thereof as route identification information in the candidate navigation network; setting all intermediate nodes between two endpoints of a corresponding candidate navigation route and the endpoint of the same candidate navigation route as traversed nodes; and the parent node of each node on the corresponding one of the candidate navigation routes is positioned at a node position adjacent to the node along the path extending direction of the starting point of the end point of the corresponding one of the candidate navigation routes.

And (4) configuring the starting point or the ending point of the candidate navigation route as a node capable of uniquely determining one candidate navigation route as an identifier corresponding to one candidate navigation route and recording corresponding parent node information, and configuring the rest nodes of the same candidate navigation route as the nodes which are repeatedly searched, so that the searching amount in the path planning process is reduced.

Further, in the step 5, the specific steps of sequentially connecting the child nodes and their parent nodes, except for all the intermediate nodes of the candidate navigation route and their corresponding one end points in the step 4, from the navigation end point based on the previously recorded position information of the parent node until the child nodes and their parent nodes are connected to the navigation start point include: step 51, starting from the navigation destination point based on the recorded position information of the father node, and connecting the navigation destination point and the father node thereof; step 52 is then entered; step 52, using the currently determined father node as the child node, and connecting the father node of the child node based on the recorded position information of the father node; step 53, repeating step 52 until connecting to one end point of the corresponding one candidate navigation route in step 4, then starting from the other end point of the corresponding one candidate navigation route in step 4, connecting to the other end point of the corresponding one candidate navigation route in step 4 and its father node, and then returning to step 52; step 54, repeating step 52 and step 53 until the navigation path is connected to the navigation starting point, and realizing reverse connection to obtain a navigation path from the navigation starting point to the navigation ending point.

Compared with the prior art, after the navigation path is planned, the technical scheme is realized: when a robot searches a path through a narrow area by using a path searching algorithm, if the narrow area is detected, switching a navigation path searched by an entering path searching algorithm near the narrow area into the candidate navigation route; if the situation that the navigation route leaves the narrow area is detected, the candidate navigation route is switched to a navigation route searched by an entering route searching algorithm; the robot can effectively pass through the narrow area, and the navigation success rate of the narrow area is improved.

Further, in the step 5, the path cost sum value is obtained by adding or weighting a traversed path cost and a predicted path cost, where the traversed path cost is a cost of a specified node from all the nodes to be traversed from the navigation start point, and the predicted path cost is a cost of the same specified node from the navigation end point; when the path cost sum value is smaller, configuring the traversing priority of the appointed node among all nodes to be traversed to be higher; wherein the movement cost between two adjacent nodes is represented using a manhattan distance, a diagonal distance, or a euclidean distance. The method is favorable for searching out the shortest path with the lowest navigation cost.

Further, starting from a navigation starting point, sequentially connecting each traversed grid center point until connecting to the designated node, acquiring the length of a connecting line formed by current connection according to the side length of the grid, and taking the length as the traversed path cost; setting the designated node as a father node, acquiring all the connection schemes from each child node corresponding to the father node to a navigation terminal point, acquiring the length of each connection scheme corresponding to each child node according to the side length of the grid, and selecting the shortest length corresponding to each child node as the corresponding predicted path cost; wherein the grid points are represented by grid center points and are used for representing the position characteristics of one grid. And the calculation mode of the path cost is simplified. The cost value calculated by the technical scheme is a measure for the cost of the motion trail of the robot, and represents the cost from the starting point to the designated node and then to the end point, including the path length.

Further, after setting the traversed node, the same node is not allowed to be configured as the node to be traversed. In order to identify the searched nodes in the planning process.

Further, the step 4 further includes: and if the free grid points which are not positioned on the corresponding candidate navigation route in the neighbor of the current father node or all the free grid points in the neighbor of the current father node are configured as the nodes to be traversed, changing the current father node from the nodes to be traversed to the traversed nodes. Repeated expansion with the same search center is prevented.

A chip for storing program code for performing said one route optimization method and/or for performing said path planning method. Based on the regional passable condition of the whole grid region, the robot is controlled to search a complete route point set which can overcome the problem that the grid cannot pass due to the drift error of the map in a normal working and moving state.

A robot incorporating said chip for performing said one route optimization method and/or said path planning method. When the robot detects the narrow area and plans to pass through the narrow area, the robot lays out a complete candidate navigation network in the current narrow area by using the candidate navigation route which is searched out completely in advance, and then plans a navigation path from the complete candidate navigation network by using a path search algorithm, thereby effectively overcoming the influence of grid marking errors caused by sensor accumulated errors (indirect reasons) and map drift (direct reasons), and passing through the narrow area without barriers along the navigation path formed by connecting the corresponding candidate navigation routes.

Drawings

Fig. 1 is a flowchart of a method for planning a route using the candidate navigation route planned in fig. 3 according to an embodiment of the present invention.

Fig. 2 is a flow chart of a method for planning a local route within a stricture in accordance with yet another embodiment of the invention.

FIG. 3 is a flow chart of a route optimization method disclosed in connection with connecting the planned partial route of FIG. 2 in accordance with another embodiment of the present invention.

Detailed Description

The following describes the embodiments of the present invention further with reference to the drawings.

It should be noted that, the general PRM algorithm operation flow: firstly, randomly generating enough number of free grid points in a grid map built immediately, and secondly, adopting a certain strategy to directly connect the generated random free grid points into feasible edges so as to form a graph, wherein the graph is a generated route map, and the random free grid points are all free nodes which are uniformly distributed in the grid map in a completely equal probability manner, wherein the defect of poor route map generation completeness is caused. The PRM algorithm is therefore different from the conventional graph search algorithm, which includes but is not limited to an a-th algorithm, a D-th algorithm, in that the roadmap is not structured in a deterministic manner in the planning space, but is structured using some random probability method based on probability theory; after the PRM algorithm generates a route map, a conventional map search algorithm such as a is utilized to perform route planning, for example, route planning or query of an optimal route, so that the PRM algorithm can find a navigation route by using relatively fewer random sampling points in an environment with fewer obstacles, and when the number of obstacles is large or the number of sampling points is too small, and the distribution of nodes in a corresponding area is unreasonable, the route planning of the nodes sampled by the PRM algorithm is likely to fail, so that the route marked by the PRM algorithm rule becomes incomplete.

It should be noted that it is understood by those skilled in the art that: the environment information around the current position of the robot is marked in the immediately constructed grid map, and the grids in the map area constructed by the robot comprise three states marked as free, occupied and unknown; these grids are represented in this embodiment using grid points, i.e. the center points of the grids; the grid points in the free state are grid points which are unoccupied by the obstacle, are grid position points which can be reached by the robot, are free grid points and can form an unoccupied area; the grid points in the occupied state are grids occupied by the barrier, are barrier grid points and can form an occupied area; the unknown grid points refer to grid areas with unclear concrete conditions in the process of constructing the map by the robot, and the position points of the unknown grid points are often blocked by barriers, so that the unknown areas can be formed.

It should be noted that, the intelligent sweeping robot in the prior art often moves to a narrow area formed by limitation among various furniture components, such as four feet of a stool in a home environment, an entrance of a tea table, and a narrow area formed by opening a door of a room. It is to be added that the narrow area is a slit channel of each obstacle, wherein the slit channel of each obstacle is a slit corresponding to the narrowest part between two obstacles, and the width of the slit is larger than the diameter of the robot body, so that the robot is allowed to pass through. Because the robot has slipping condition, the sensor for positioning has accumulated error or the visual map optimization also generates error, the robot easily marks the narrow area as the area occupied by the obstacle on the grid map constructed by the instant scanning, namely the free grid points originally mapped by the narrow area are incorrectly marked as the area occupied by the obstacle, so that the entrance of the narrow area mapped to the grid map constructed by the robot is blocked, the actual movement scene is not really blocked, the robot cannot complete the planning of a navigation path penetrating through the narrow area by using a mature path searching algorithm, and the robot cannot sample enough free grid points in the area with complex topography of the narrow area by using a PRM algorithm so as to facilitate path planning.

Therefore, in order to solve the technical defects, the invention discloses a route optimization method, a route planning method, a chip and a robot, wherein the robot sequentially uses free grid points randomly generated and starting and stopping points of a local route as search centers on the basis of searching and fitting a local route on a motion track in a narrow area, and searches nearest free grid points which can pass through in an open map area layer by applying a PRM algorithm under different screening conditions, so that the sampling number of the free grid points distributed in a difficult area is increased, and the planned feasible route is connected with all the free grid areas of the local route to be covered on the corresponding map area as much as possible, thereby forming a candidate navigation route network with more completeness.

As an embodiment, as shown in fig. 3, a route optimization method is disclosed, which specifically includes the following steps:

step S301, controlling a robot to move along a preset path and planning at least one local route; and then proceeds to step S302. The preset path is a preset path for supporting the robot to pass through the narrow area. Whether a local route or a preset route, the route nodes connected into the routes are represented by grid points; when the robot moves on the preset path, the robot traverses the narrow area to perform corresponding route searching in the normal working process, and in the same process, the robot moves along the preset path while collecting path nodes conforming to traffic conditions or assisting traffic, local routes supporting the robot to enter and exit the narrow area are planned near or in the narrow area, the number of the planned local routes is not limited, but the local routes are mutually connected into a route map, namely a route network in a grid map.

Step S302, randomly searching a first preset number of grid points meeting a first communication screening condition in a grid map constructed by the robot; and then proceeds to step S303. The grid map constructed by the robot is a grid map constructed in real time in the process from starting to executing the step S302 of the robot, or a grid map constructed by scanning an open area in advance, wherein the open area is an area except a narrow area, an obstacle area and a normal walking area. Preferably, the grid map constructed by the robot disclosed in the present embodiment is the maximum grid map recorded by the robot.

In step S302, the robot randomly searches a first preset number of grid points meeting a first connection screening condition in the open area, which specifically includes:

calculating a first ratio of the length of the constructed grid map of the robot to the side length of the single grid, and rounding the first ratio, namely discarding a decimal part of the first ratio, and reserving an integer part of the first ratio to obtain the number of grids which can be contained at most in the transverse axis direction (X axis direction) of the constructed grid map; meanwhile, calculating a second ratio of the width of the constructed grid map of the robot to the side length of the single grid, and rounding the second ratio, namely discarding a fraction part of the second ratio, and reserving an integer part of the second ratio to obtain the number of grids which can be contained at most in the longitudinal axis direction (Y-axis direction) of the constructed grid map; then, carrying out product operation on the rounding result of the first ratio and the rounding result of the second ratio, wherein the product result is used as the total number of grids distributed in the grid map constructed by the robot and is determined as a first preset number, and the distribution density of the nodes in the grid map can be expressed; wherein the grid points are used for the grid center point representation of a corresponding one of the grids.

And randomly searching grid points meeting the first communication screening condition in the grid map by taking the current position of the robot as a search center, and adding a pre-established feasible route set until the first preset number of grid points meeting the first communication screening condition are randomly searched and obtained, wherein in order to obtain the first preset number of grid points meeting the first communication screening condition, the random search is repeatedly executed in the grid map for at least the first preset number of times. The random search here is: searching each grid in the grid map in a completely equal probability mode, specifically, randomly searching a random integer coordinate x in the transverse axis direction between a maximum transverse axis coordinate and a minimum transverse axis coordinate of the grid map, randomly searching a random integer coordinate y in the longitudinal axis direction between a maximum longitudinal axis coordinate and a minimum longitudinal axis coordinate of the grid map, generating a coordinate c (x, y) of a node to be screened, wherein the maximum value of x is equal to the maximum transverse axis coordinate, the minimum value of x is equal to the minimum transverse axis coordinate, the maximum value of y is equal to the maximum longitudinal axis coordinate, the minimum value of y is equal to the minimum longitudinal axis coordinate, and the maximum coordinates are integer values and accord with the characteristic of the discrete map of the grid map; screening the randomly searched nodes to be screened according to a first communication screening condition, and adding grid points meeting the first communication screening condition into a feasible route set; the feasible route set is used as a node set and is used for storing path nodes meeting different screening conditions, but the same path node cannot be repeatedly stored, wherein a feasible route is generated between certain two nodes. For convenience of description, the present embodiment sets the grid points satisfying the first connection screening condition as the first connection nodes, that is, the grid points satisfying the first connection screening condition are stored inside the feasible route set in the form of the first connection nodes.

Specifically, the first connection screening condition is: and the free grid point of the current random search does not exist in the feasible route node set, and no barrier grid point and no unknown grid point exist in the area with the free grid point of the current random search as a circle center and the search radius as one body diameter, so that the free grid point of the current random search is the first communication node. The method has the advantages that no obstacle blocking the robot to pass is arranged in the explorable area near the first communication node, and the first communication node which is searched randomly in the constructed grid map has more passing significance. And introducing grid points which are randomly generated in the open area and meet the first communication screening condition into the narrow area, enhancing the number of free grid points in the whole grid map area, and improving the distribution density of the first communication nodes in the grid map. According to the method, the total number of grids distributed in the grid map constructed by the robot is used for representing the distribution density of the nodes in the grid map, and then the first communication nodes used for representing the total number of the grids distributed in the grid map are randomly searched, so that the distribution density of free nodes in a grid map area is improved, the minimum value of the number of free grid points in an open area is ensured, the generated candidate navigation network is optimized, and the completeness of the candidate navigation network is improved.

As an embodiment, between executing step S302 and step S303, the local route is further stored in the feasible route set, and the specific steps include:

setting the starting point and the ending point of the local route as second communication nodes; meanwhile, on the local route, setting an intermediate node between a starting point and an ending point of the local route as a third communication node; the starting point, the ending point and the intermediate node of the local route are all stored in a predicted passing node set; the predicted passing node set is created in the process that the robot moves along the preset path. Then, in the embodiment, the position information of the child node of the second communication node is recorded in the predicted passing node set, and the second communication node is added into the feasible route node set; recording the position information of the child node of the third communication node in the same predicted passing node set, and adding the third communication node into the feasible route node set; so that the feasible route node set stores one complete path node which is used for representing a corresponding unique local route in the prediction passing node set; the predicted passing node set is created in the process that the robot moves along the preset path; wherein, whether joining the predicted transit node set or the feasible route node set, the second connected node representing the same local route and its child nodes are adjacent and connected; a third connected node representing the same local route and its children are adjacent and connected. Each path node stored in the predicted transit node set and its child nodes collectively represent a unique one of the local routes in the local route node set. And the path nodes of the local route planned by the robot are transferred from the predicted passing node set to the feasible route node set one by one, so that the original local route is restored in the feasible route node set in a connecting mode based on the recorded position information of the child nodes.

In this embodiment, when the second communication node and the third communication node join the feasible route node set, the second communication node and the third communication node in the feasible route node set are sequentially connected to form the local route according to the adjacent relationship between the child node and the parent node thereof based on the recorded position information of the child node. Specifically, the embodiment uses the second communication node as a starting point, expands a new node according to a preset step length to expand the third communication node, uses the third communication node which is currently expanded as a new starting point, continues to expand the subsequent third communication node until the third communication node is expanded to the new second communication node, and sequentially connects the complete local routes according to the sequence of sequential expansion. The local routes originally stored within the set of predicted transit nodes are completely restored.

Step S303, respectively taking the grid points meeting the first communication screening condition, the starting points of all local routes and the end points thereof as a first search center, searching out the grid points meeting the second communication screening condition, and then connecting the first search center and all the searched grid points meeting the second communication screening condition into a first feasible route, thereby determining that the first feasible route is connected with the local route; and then proceeds to step S304.

Step S303 specifically includes: the first communication node and the second communication node are respectively used as the first search center, grid points meeting the second communication screening condition are searched in the feasible route node set, namely the first communication node is used as the search center, the grid points meeting the second communication screening condition are searched in the existing elements of the feasible route node set but are not necessarily limited to the neighborhood for expansion, and the second communication node is used as the search center, and the grid points meeting the second communication screening condition are searched in the existing elements of the feasible route node set but are not necessarily limited to the neighborhood for expansion; setting grid points meeting second communication screening conditions as first legal sub-nodes of corresponding first search centers and adding the first legal sub-nodes into sub-node sets of the corresponding first search centers, wherein the sub-node sets of the corresponding first search centers also exist in the feasible route node sets, are attached to the first search centers and exist in a structural form, and enable each first communication node and each second communication node to have a corresponding sub-node set in the feasible route node sets; based on the data structure, the embodiment connects the same first search center and all first legal sub-nodes as the first feasible route, the first search center and each first legal sub-node can be mutually connected to increase the completeness of route distribution in the area, and preferably, each first legal sub-node in the sub-node set of the first search center is not connected with a third communication node added in the feasible route node set; further, a first feasible route is determined to be connected with the local route, but the first feasible route is limited to be connected with a starting point and a stopping point of the local route. In the embodiment, grid points meeting the second communication screening condition are selected in the surrounding area of the start and stop points of the local route and the surrounding area of the first communication node, so that the situation that the neighborhood of each free grid point is focused and child nodes are added in the neighborhood like a traditional PRM algorithm is avoided, and the searching efficiency is low; on the other hand, compared with searching for the grid points satisfying the second communication screening condition only in the narrow area (the area represented by the area around the start-stop point of the local route), the present embodiment can screen out the grid points satisfying the second communication screening condition in a wider area and record as legal child nodes for connecting the local route, which has the advantage that by fusing the local route with the first feasible route, a plurality of routes for connecting the narrow area, the open area, the normal area and the obstacle area are generated, and the connectivity of the candidate navigation network in the narrow area and the open area is improved.

In the above embodiment, the second communication screening condition includes: the grid points which do not have barrier grid points on the connecting line with the first search center and the connecting line length with the first search center is smaller than a preset distance threshold value are the first communication nodes or the second communication nodes; the preset distance threshold is preset according to specific environmental conditions, and specific numerical values can be different due to different environmental conditions of a narrow area and an open area; in this embodiment, the preset distance threshold is set to be greater than the side length of a single grid, and nodes that can be connected into a valid first feasible route are screened out. Therefore, the grid point meeting the second communication screening condition is a first communication node or a second communication node which has no barrier grid point on the connection line with the first search center and the connection line length with the first search center is smaller than a preset distance threshold value. In this embodiment, the distance threshold concept is set up to enable the number of free grid points in a certain area to reach a certain value, so that free grid points exist in a corresponding distance range of the first search center in a discrete grid area corresponding to the feasible route node set, and the situation that the number of free grid points in the area is not too small to generate a first legal child node meeting the requirement is avoided. So as to ensure that the number of free grid points in the area is not too small to generate a road map meeting the requirements to form a navigation network.

Step S304, judging whether the grid points meeting the first communication screening condition exist in all the child nodes of the starting points of all the local routes and all the child nodes of the ending points of all the local routes, if yes, proceeding to step S305, otherwise proceeding to step S306. The starting points and the ending points of all the local routes are all nodes in the neighborhood of the corresponding nodes in the grid map, which are already added into the feasible route node set, and then are judged according to the child nodes stored in the child node set of the corresponding nodes in the feasible route node set.

Step S305, determining that a first feasible route is connected with the local route to form a complete candidate navigation network, namely, the two routes are integrated completely; the method specifically comprises the following steps: if the fact that all the sub-nodes of the second communication nodes in the feasible route node set are all provided with the first communication nodes is detected, determining that two ends of a local route to which all the second communication nodes belong are respectively connected with all the first communication nodes in the sub-node set added into the corresponding first search center, and further determining that the local route and the first feasible route are connected to form a complete candidate navigation network; the child nodes of the second communication node detected in step S304 are in the child node set corresponding to the same second communication node. Compared with the prior art, the embodiment discloses a first communication node randomly searched in the grid map representing the open area and a planned local route in the narrow area, and after the reasonable number of free grid points and the communication environment are provided, a complete candidate navigation network is finally formed, so that the defect of poor completeness of a generated feasible route caused by completely and uniformly distributing the free grid points in equal probability of a general PRM algorithm is overcome, and the feasible route passing through the narrow area and the open area is not easy to miss.

And step S306, respectively taking the starting points and the end points of all the local routes as second search centers, searching out grid points meeting the third communication screening condition, and connecting the second search centers and all the searched grid points meeting the third communication screening condition into a second feasible route. Specifically, with respect to step S303, step S306 searches for a grid point satisfying a third connectivity screening condition from the existing elements in the feasible route node set only with the second connectivity node as a search center, but is not limited to expansion in the neighborhood, and the third connectivity screening condition is different from the second connectivity screening condition; setting grid points meeting third communication screening conditions as second legal sub-nodes of the corresponding second search centers and adding the second legal sub-nodes into sub-node sets of the corresponding second search centers, wherein the sub-node sets of the corresponding second search centers also exist in the feasible route node sets, are attached to the second search centers and exist in a structural form, and enable each second communication node to have a corresponding sub-node set in the feasible route node sets; based on the data structure, the embodiment connects the same second search center and all second legal sub-nodes as the second feasible route, the second search center and each second legal sub-node (including each node in the neighboring domain) may be connected to each other to increase the feasible routes already generated in the same area in step S303, so as to improve the completeness of the generated routes, and preferably, each second legal sub-node in the sub-node set of the second search center is not connected to a third communication node added in the feasible route node set; and further determining that the first feasible route and the second feasible route are connected with the local route to form a more complete candidate navigation network, wherein the second feasible route is only connected with the starting point and the ending point of the local route. The child nodes of the second communication node detected in step S304 are in the child node set corresponding to the same second communication node. Therefore, in this embodiment, the search range is narrowed, elements of the sub-node set corresponding to the second connected node are increased, and the probability of distribution of free grid points in the neighborhood of the second search center is relatively increased, relative to searching out grid points that meet the second connected screening condition. The probability of missing a viable path through the difficult region is reduced.

The third connectivity screening condition includes: among all grid points which have no obstacle grid points and no unknown grid points on the connection line with the second search center, the grid point with the shortest connection line length with the second search center is a first connection node. Namely, the grid points meeting the third communication screening condition are: and a first communication node which has no barrier grid point and no unknown grid point on the connection line with the second search center and has the shortest connection line length with the second search center. In the map in the area with insufficient free grid points, the passable path nodes with the shortest distance are searched out by increasing the acquisition point quantity near the start and stop points of the local route, and the connectivity standard between the nodes is reduced, so that the path search of an algorithm in a narrow channel is enhanced, and the method is more suitable for complex terrains. And compared with the requirement of the second communication screening condition, the search area matched with the third communication screening condition is reduced, and the speed of route planning is improved.

In this embodiment, in the complete candidate navigation network, two ends of the local route are respectively used to connect an end point of the first feasible route and/or an end point of the second feasible route, but neither the first feasible route nor the second feasible route is connected to an intermediate node of the local route; so as to search a route from a preset navigation starting point to a preset navigation ending point in the complete candidate navigation network by using a path search algorithm; the candidate navigation network is a complete navigation route map which is constructed by executing the steps S301 to S306 and can be repeatedly used by a path searching algorithm. It should be noted that, in the foregoing embodiment, the narrow area is a slit channel of two or more obstacles, the slit channel is a slit corresponding to the narrowest portion between the two obstacles, and the width of the slit is greater than or equal to the diameter of the body of the robot; wherein the node set is a data structure occupying memory space. The open area represents a grid map area that the robot has constructed, may include an open area near a narrow area, where island obstacles alone are insufficient to block the robot's normal travel, or where the obstacle distribution is relatively discrete such that the distance formed between the obstacles is a preset multiple of the diameter of the fuselage, the preset multiple being specifically configured according to the indoor environment in which the robot is located. In this embodiment, the random free grid points generated by the foregoing steps are connected to two ends of the local route to form a graph, where the graph is the generated candidate navigation network capable of covering the narrow area and the open area, and is used as a search source of the relatively complete candidate navigation route in the narrow area by the path search algorithm, so that an optimal navigation path more meeting the requirements is better found, and the probability of missing a feasible path passing through the difficult area is reduced.

Another embodiment of the present invention discloses a method for planning a local route, as shown in fig. 2, specifically including:

step S201, in the process of controlling the robot to move along the preset path, judging whether the pre-search area meets the first preset round-domain passing condition in real time, if yes, entering step S202, otherwise, controlling the robot to continue to move along the preset path. The preset path is a path which is planned in advance by the robot; the path node support is represented by grid points; the preset path is a normal working path or a navigation path of the robot, when the robot is a sweeping robot, the preset path can be an arcuate moving path, a edgewise walking path, a back-shaped path and the like, and the robot moving on the preset path can perform route coordinate points or route searching suitable for crossing the narrow area in the normal working process or perform corresponding route searching in the process of entering the narrow area. The pre-search area is a first circular area with the current position of the robot as the center and the diameter of the robot body as the radius, and covers the minimum passable area around the robot.

Preferably, in the process of executing the step S201, the method further includes: firstly judging whether the robot moves to the end point of the preset path, if so, stopping executing the planning method of the local route, otherwise, controlling the robot to continuously move along the preset path, and judging whether the pre-search area meets a first preset round-domain passing condition in real time; step S201 timely prevents the robot from continuing to move and executing the planning method of the local route, so as to meet the intelligent planning requirement, and avoid the robot from searching route coordinates without limit, thereby causing excessive consumption of computing resources.

Step S202, recording the current position of the robot as a first path node, simultaneously creating a new predicted passing node set, storing the first path node into the predicted passing node set, and then entering step S203. In this embodiment, the elements within the set of predicted passing nodes are configured to store the first path nodes in a recorded chronological order.

It should be noted that, the first element and the tail element in the predicted passing node set are all unique, so that the first element or the tail element in the predicted passing node set becomes the unique identification information of the route represented by the first element or the tail element, and can be used as an index node in the process of planning a path or as a marking node of a trace-back path.

In this embodiment, the first preset round-field traffic condition includes: the area ratio of the first non-passable area in the pre-search area is larger than the first pass evaluation value; the first non-passable area is a grid area consisting of unknown grid points and barrier grid points in the grid area corresponding to the first circular area; the first traffic evaluation value is a pre-judgment threshold value set for overcoming a marking error of a free grid existing in a constructed grid map, and is a result of repeated experiments in the narrow area; the first pass evaluation value is preferably set to 50%. When the robot judges that the area proportion occupied by the first non-passable area in the grid area corresponding to the first circular area is less than or equal to 50%, the robot is controlled to move along a preset path until the robot judges that the area proportion occupied by the first non-passable area in the grid area corresponding to the first circular area is greater than 50%. Therefore, the first preset circle passing condition is used for primarily judging whether the robot starts to enter the narrow area, belongs to a rough judging condition, and further judgment is carried out in the continuous moving process of the robot. But without regard to whether the marking information of each grid connected to the planned path allows the robot to pass through, the effect of individual grid marking errors is reduced.

Step S203, when the robot is controlled to continue moving along the preset path, whether the robot moves to a position with a straight line distance greater than or equal to the diameter of the body of the robot from the latest recorded path node is judged in real time, if yes, step S204 is entered, otherwise, the robot is controlled to continue moving along the preset path. The linear distance between the robot and the last recorded path node detected in step S203 includes the linear distance between the current position of the robot and the first path node, where the robot keeps detecting the linear distance between the current position and the last recorded path node in real time, or may perform distance sampling detection at regular intervals of detection periods, and record the path node detected in real time when a certain traffic condition is satisfied.

In the process of executing step S203, further includes: judging whether the robot moves to the end point of the preset path, if so, stopping executing the planning method of the local route, otherwise, controlling the robot to continue to move along the preset path, and judging whether the robot moves to a position with the straight line distance from the latest recorded path node being greater than or equal to the diameter of the body of the robot. Therefore, the robot is prevented from continuously moving and executing the planning method of the local route in time, so that the intelligent path planning requirement is met, and the situation that the robot moves limitlessly to continuously search route coordinates, so that excessive consumption of calculation resources is caused is avoided.

And step S204, when the linear distance between the current position of the robot and the newly recorded path node is detected to be greater than or equal to the diameter of the body of the robot, recording the current position of the robot as a second path node, and then entering step S205.

Step S205, judging whether the second path node meets a second preset round domain passing condition, if yes, entering step S206, otherwise, entering step S207; thus, the last recorded path node in step S204 includes the last recorded first path node or the last recorded second path node. The second path node recorded in step S204 is a new second path node with respect to the second path node recorded in the last execution of step S204, or the second path node recorded in step S204 is a new path node with respect to the first path node recorded in the last execution of step S202.

As one embodiment of the second preset round-domain traffic condition, the second preset round-domain traffic condition includes: in a second circular area taking the current position of the robot (namely a newly recorded second path node) as a circle center and the diameter of the body of the robot as a radius, the area proportion occupied by a second non-passable area is larger than a second passable evaluation value; the second non-passable area is a grid area consisting of unknown grid points and barrier grid points in the grid area corresponding to the second circular area; the second pass evaluation value is a judgment threshold value set for overcoming a marking error of a free grid existing in the construction grid map, and is larger than the first pass evaluation value to improve judgment accuracy. The second pass evaluation value is preferably set to 75%. When the robot judges that the area ratio occupied by the second non-passable area in the grid area corresponding to the second circular area is less than or equal to 75%, the step S207 is entered. Wherein the coverage area of the second circular area is different relative to the coverage area of the first circular area because the position of the robot changes; therefore, on the basis of the first circular area or the basis of the last searched second circular area, after the robot moves to the diameter of the robot body which is one distance from the latest recorded path node, the judgment of the area proportion occupied by the second non-passable area is further carried out, the influence of the marking information is not considered one grid point by one grid point, and the judgment precision of the narrow area is improved. In this embodiment, only the exploration radius of the second circular area is set to be the diameter of the robot body, instead of a larger value, so that the control-irrelevant (out of the exploration range) grid area is prevented from participating in calculation, and the time cost is reduced.

As another embodiment of the second preset round-domain traffic condition, the second preset round-domain traffic condition includes: searching a path of the newly recorded second path node to the newly recorded first path node by using a path searching algorithm in a second circular area taking the newly recorded first path node as a circle center and taking the diameter of the machine body as a radius, wherein the preset multiple of the machine body diameter is used for proving that the path searching in the current second circular area is not influenced by an obstacle or an error mark of an obstacle grid; since the second circular area may be in the vicinity of the narrow area, there may be an obstacle between the newly recorded second path node and the newly recorded first path node, and the node that may collide is called an invalid node or an illegal node in the embodiment, and these nodes need to be avoided when planning a path, so that it is necessary to use a mature stable path search algorithm to check the feasibility of the area, and the path search algorithm used in the embodiment is an a-x algorithm, so that the navigation path from the newly recorded second path node to the newly recorded first path node is effectively and quickly searched in the second circular area. In this embodiment, the first circular area or the second circular area may be regarded as a cleaning area. Wherein, the fuselage diameter of predetermineeing the multiple sets up to: the second circular area is not intersected with other marked grid areas, so that the planned paths in the other marked grid areas are prevented from participating in judgment, and misjudgment is avoided. The predetermined multiple of the fuselage diameter is set to two-thirds of the fuselage diameter such that a second circular area is formed that is larger than the first circular area. Compared with the prior art, the size of the search area, namely the second circular area, is set reasonably, and the situation that the robot is guided to other areas and is not guided to pass through the current narrow area due to the fact that the robot intersects other known map areas to add the planned route in the relevant area is avoided; on the other hand, whether a complete navigation path from a starting point to an end point can be planned in the second circular area currently explored is judged, so that the passable area of the second circular area is not influenced by the obstacle or the marking position of the obstacle grid; thereby improving the judgment accuracy of the narrow region.

Step S206, judging that the robot is currently in the narrow area, adding a second path node into the predicted passing node set in the step S202, and returning to the step S203; since the robot continues to move along the preset path within the stenosis by returning to step S203 after recognizing that its current position is within the stenosis. But the robot may have moved to the end of the preset path, it is preferable to control the robot to proceed from step S206 to step S208 for further path node judgment.

Therefore, in this embodiment, after the robot searches the first path node, when the linear distance of one machine body diameter is moved, that is, when the linear distance between the current position and the position of the last recorded path node is the machine body diameter, it is determined whether the current position or the current search area of the robot meets the second preset circle domain passing condition; and then, when the fact that the robot is currently located in the narrow area is determined through the second preset circle domain passing condition, continuing to move in the narrow area along the preset path, and continuing to search out a new second path node to connect a route for passing through the narrow area until the fact that the second path node does not meet the second preset circle domain passing condition is judged. The narrow area is a slit channel of two or more obstacles, the slit channel is a slit corresponding to the narrowest part between the two obstacles, and the width of the slit is larger than or equal to the diameter of the robot body.

Step S207, judging that the robot is not currently located in the narrow area or just leaves the current narrow area to come to an unoccupied area with smaller area, simultaneously storing the predicted passing node set into the same local route node set according to the number of path nodes stored in the predicted passing node set, and connecting the path nodes stored in the predicted passing node set into a corresponding local route according to the sequence of adding, wherein the step is equivalent to adding the end point of the corresponding local route into the predicted passing node set, so that one predicted passing node set is formed into a node set representing one local route in the local route node set; then, returning to the step S201, controlling the robot to carry out a round of judgment on the narrow area again at a new position, namely, iteratively executing the steps S201 to S207 to create a new predicted passing node set in the local route node set so as to describe a new local route; but the robot may have moved to the end of the preset path, it is preferable to control the robot to proceed from step S207 to step S209 for further path node judgment.

Therefore, step S208 is executed to determine whether the robot moves to the end point of the preset path, if so, the local route planning method is ended, otherwise, step S203 is executed.

Step S209, determining whether the robot moves to the end point of the preset path, if yes, ending executing the planning method of the local route, otherwise returning to step S201. Therefore, the robot is prevented from continuously moving and executing the planning method of the local route in time, and the situation that the robot performs route planning without limitation, so that excessive consumption of calculation resources is caused is avoided.

It should be noted that, within a predicted passing node set, it is allowed to include an obstacle grid point, which indicates that the corresponding local route is likely to have an obstacle grid point. As is known from steps S201 to S209, the robot itself moves to a specific path node, for example, the current position of the robot corresponding to step S202 (the initial state condition is satisfied) during one iteration, or the current position of the robot corresponding to step S206 during one iteration, and when it is determined that the first preset round-domain traffic condition or the second preset round-domain traffic condition is satisfied on the path node, the path node is added to the predicted traffic node set as a path node connected to the corresponding local route, however, due to the sensor detection error, the map drift error, etc., the marked grid information of the path node on the grid map may not be the free state, but may be mismarked as the obstacle occupying state, that is, the obstacle grid point, the path node is also added to the predicted traffic node set as a path node on the corresponding local route, and the actual robot may move to the path node, which proves that the path node is passable.

It is noted that, inside the same local route node set, there are a plurality of the predicted passing node sets, each representing other mutually different local routes, because inside the same local route node set, the head element and the tail element inside any one of the predicted passing node sets are unique.

Specifically, according to the number of path nodes stored in the predicted passing node set, the method for storing the predicted passing node set into the same local route node set specifically includes: when the number of path nodes stored in the predicted passing node set is smaller than 2, it indicates that the robot may detect an obstacle that is difficult to surmount or is trapped or has other abnormal conditions, so that the path nodes stored in the predicted passing node set are invalid nodes and represent invalid local routes, the predicted passing node set is selected to be deleted, and then step S201 is returned to create a new predicted passing node set. Therefore, in this embodiment, if the number of path nodes finally obtained by one predicted transit node set is so small that it is difficult to connect out a line, the relevant set may be deleted to reduce invalid path nodes.

When the number of path nodes stored in the predicted passing node set is greater than or equal to 2, the predicted passing node set is used for representing a single local route and is stored in the local route node set so as to be called by a path searching algorithm, which can be a heuristic searching algorithm, and a new predicted passing node set is created by returning to the step S201; preferably, the path nodes finally acquired by one prediction passing node set can be saved as a whole route to the local route node set, so that the stored data of the local route are reasonably ordered. The first path node and the second path node are added into the predicted passing node set according to the recorded sequence, so that the path nodes stored in the predicted passing node set are connected into a corresponding partial route according to the recorded sequence. It should be noted that, the path node corresponding to the first element in each predicted passing node set is the start point of a corresponding local route, and the path node corresponding to the tail element in each predicted passing node set is the end point of a corresponding local route. The path nodes in each predicted passing node set are matched and connected to form a local route, so that a local route for predicting the robot to pass without obstacle in a corresponding area is formed;

Compared with the prior art, the method for planning the local route in the steps S201 to S207 is suitable for implementing route planning in a state that the robot enters the narrow area, and first, a first preset round-domain traffic condition is set as a pre-judging condition of the narrow area, and a path node source of the local route corresponding to the predicted traffic node set is provided; and then the robot is enabled to continuously move along the original path, a second preset circle passing condition is set as a precise judging condition of the narrow area, and the grid point sources of the local routes corresponding to the predicted passing node set are continuously provided, so that the robot can be qualified for judging the narrow area after moving a certain distance, the local routes which are collected by the predicted passing node set and are used for being connected into grid points are more complete, the map grid error environment under the narrow area is more suitable, and the practically passable routes are provided for the robot without paying attention to the grid mark information on the routes one by one. In addition, on the basis, if the corresponding preset round-domain passing condition is not met, stopping searching the path nodes for the predicted passing node set, and determining that the path nodes in a single predicted passing node set can be connected into an independent local route. By iteratively executing the foregoing correlation steps, the robot searches for a set of route points that can overcome map drift errors in a state of normal operation movement. The success rate of the robot for finding the effective navigation path in the scene with more complex obstacle space layout is increased.

Preferably, if the robot currently detects that the robot is not located in the narrow area, the robot starts to execute the route optimization method of the previous embodiment by using a plurality of the predicted passing node sets in the local route node sets, so as to plan a local route supporting the robot to freely enter and exit the narrow area, and overcome map drift errors. The problem that the free grid points searched in the narrow area are easily marked as barrier grid points, so that a planned path cannot pass through the narrow area is solved.

Preferably, if the current position of the robot is surrounded by an obstacle, and it cannot continue to move along the preset path, the method for planning the local route and the method for planning the local route mentioned in the foregoing embodiments are stopped.

As an embodiment, the embodiment of the invention discloses a path planning method, which needs to control a robot to execute the path optimization method of the previous embodiment to plan a candidate navigation path for searching, and then uses a heuristic search algorithm (mainly a path node search idea of the search algorithm) and the candidate navigation path to plan an integral navigation path, so as to overcome the marking error of a map grid, and solve the navigation path planning problem in a passable area where the robot has a narrower passage in the grid map and is easy to generate marking error and the technical problem that the robot freely passes through a narrow area. The path planning method may be implemented after the robot has completely left the stenosis or just before entering a new stenosis after leaving the current stenosis.

The path planning method comprises the following steps:

firstly, executing the route optimization method disclosed in the previous embodiment; after the route optimization method is performed, the first feasible route, the second feasible route and the local route are all configured as candidate navigation routes in the candidate navigation network. The more the number of candidate navigation routes formed in the candidate navigation network is, the higher the coverage rate of the map area is, namely the better the completeness of the candidate navigation routes is.

After the route optimization method is executed and the candidate navigation route is configured, the route planning method further comprises the following steps:

step S101, setting a navigation starting point and a navigation end point in the grid map, and creating a node cache space to be traversed; and then proceeds to step S102. The grid map is a local map which is constructed by the robot and comprises a starting point and an ending point, further comprises obstacle information of various layouts, and comprises enough space areas so that the robot can plan a path track according to the navigation starting point and the navigation ending point. The navigation start point and the navigation end point may be represented by grid coordinates of a grid map, grid center point coordinates, or other types of navigation data, which are not limited, but can be converted into the grid map to participate in path planning. On the other hand, step S101 creates a node cache space to be traversed, initializes to be empty, and the priority of the internal element is related to the path cost, and the priority of the element may be the traversing priority or the planning priority.

Step S102, setting a navigation starting point as a current father node, specifically setting a grid or grid point where the navigation starting point is located as the current father node, and setting the current father node as a node to be traversed for selection in a subsequent neighborhood searching process or selecting to add the current father node to be traversed into a caching space; then, the process advances to step S103. In an alternative embodiment, before adding the navigation start point to the node cache space to be traversed, the initial node cache space to be traversed may be empty, and the navigation start point is the first node to be added to the node cache space to be traversed. Preferably, a parent node located at the origin of the robot coordinate system (origin of the local coordinate system) and child nodes which are searched out by expansion in the four quadrant regions are all adjacent positional relationships.

The node cache space to be traversed is regarded as a set of candidate path nodes of the path planning in this embodiment, and path nodes possibly passed by the robot are added to the set of candidate path nodes for subsequent screening processing. The node cache space to be traversed can record the position of each path node inside and the state that the robot moves to the corresponding path node. In the present exemplary embodiment, for a path node in the grid map, the state when the robot moves to reach the path node is represented by a state parameter, and the state parameter of the path node at least includes: the moment, i.e. the moment when the robot reaches the path node; furthermore, the status parameters may further include: the gesture comprises a direction when the robot reaches the path node, a steering angle (namely, a state that the robot is in straight or steering, and a steering angle) and the like; speed, i.e. the speed of movement of the robot when it reaches the path node. When path planning is started, a state parameter of a navigation starting point may be determined first, where a current moment or zero moment may be taken as a moment of the navigation starting point, and a current motion state of the robot is taken as a gesture and a speed corresponding to the starting point, for example, when the start of the robot is a stationary state, both a steering angle and a speed corresponding to the navigation starting point are 0. In an exemplary embodiment, the node to be traversed cache space is a data structure of a priority queue, and the corresponding nodes to be traversed are added in a first-in first-out storage sequence and stored in a storage space of the robot.

Step S103, performing neighborhood search in the grid map by taking the current father node as a search center, wherein 8 grid points adjacent to the current father node are respectively used as child nodes of the current father node; and then proceeds to step S104. Wherein the process of searching for child nodes on the neighborhood from the parent node (the current parent node described in step S103) may be referred to as "expansion". In the one-time expansion process, the search can be performed on the adjacent areas of the current father node in a clockwise direction (adjacent area search) or on the adjacent areas of the current father node in a counterclockwise direction (adjacent area search) to search out the child nodes meeting the relevant conditions.

Step S104, judging whether the child node searched in the neighborhood in the step S103 belongs to one endpoint of a corresponding candidate navigation route in the candidate navigation network searched in advance, if yes, proceeding to step S105, otherwise proceeding to step S106. The judging step S103 is to judge whether the child node searched in the neighborhood belongs to a starting point or an ending point representing the candidate navigation route in the candidate navigation network searched in advance, so as to connect the planned navigation route with the searched candidate navigation route for actual navigation and walking, without considering the influence of the error of the marking information on the grid point detected in real time on the navigation route planned by the conventional mature route searching algorithm, especially, marking the free grid point as the barrier grid point by mistake, so that the navigation route planned by the conventional mature route algorithm cannot pass through the barrier grid point marked by mistake, and the robot cannot pass through the narrow area. When one end point of the candidate navigation route is a starting point, the other end point of the candidate navigation route is an end point; when one end point of the candidate navigation route is an end point, the other end point of the candidate navigation route is a start point.

Step 105, setting the other end point of the candidate navigation route to which the child node searched in step 104 belongs as a node to be traversed, optionally adding the node to be traversed cache space, setting all the free grid points on the candidate navigation route which is not corresponding to step 104 and is searched in step 103 as the node to be traversed, optionally adding the free grid points into the same node to be traversed cache space created in step 101, and simultaneously recording the position information of the father node added into the node to be traversed, preferably storing the position information of the path node and the position information of the corresponding father node into the node to be traversed cache space; setting in the currently executed step S105 that the parent nodes of the rest path nodes are the current parent nodes described in the step S103 except for the other end point of the one candidate navigation route; preferably, the parent node of the other end point of the candidate navigation route is an adjacent node on the candidate navigation route or an adjacent node outside the candidate navigation route, so that the position distribution information of the candidate navigation route can be traced back through the parent node when the end point is searched, and the speed of path planning is increased.

Wherein, the reason why one of the endpoints of the one candidate navigation route to which the navigation route belongs is selected instead of the two endpoints and the intermediate node between the two endpoints is set as the node to be traversed is that: the method comprises the steps of searching a candidate navigation route in advance, searching out a relevant coordinate point set before executing the path fusion planning method, and sequentially connecting the relevant coordinate point set into path segments, wherein the identification function of the corresponding route or the index serving as a corresponding starting point on the route can be achieved only by using one end point of the candidate navigation route, therefore, the step only uses the other end point (equivalent to a non-searched path node) on the candidate navigation route to which the one end point searched by a neighborhood belongs to be set as the node to be traversed, and then the new path node with the passing meaning associated with the candidate navigation route can be continuously searched by taking the node to be traversed as the center. As for the free grid point belonging to the neighborhood of the current parent node, which is not on the corresponding candidate route in step S104, the free grid point having the shortest path basis is provided.

Therefore, in the process of executing step S107, all intermediate nodes between two end points of one candidate navigation route to which the child node searched for in step S104 belongs and the end point searched for in step S104 are set as traversed nodes. Preferably, the embodiment also creates a traversed node cache space specially for this purpose, for storing traversed nodes; it should be noted that, after the node is set to the traversed node, the same node is not allowed to be configured as the node to be traversed, that is, the node existing in the traversed node cache space is not allowed to join the node cache space to be traversed, so that the traversed node is identified or regarded as the searched node in the planning process, and repeated searching processing is avoided. Wherein, step S105 and step S107 may be performed simultaneously. After step S107 is performed, the routine proceeds to step S108.

Preferably, if the free grid point in the neighboring area of the current parent node, which is not located on the corresponding candidate navigation route, or all the free grid points in the neighboring area of the current parent node are configured as the nodes to be traversed, the current parent node is changed from the nodes to be traversed to the traversed nodes. Preferably, if all the free grid points in the neighbor of the current parent node, which are not located on the corresponding candidate route, or all the free grid points in the neighbor of the current parent node are added to the node cache space to be traversed, the current parent node is removed from the node cache space to be traversed, and then the current parent node is added to the traversed node cache space, so that the phenomenon that the same search center is repeatedly expanded is prevented, because the current parent node has already been expanded once in step S103. The traversed node cache space may exist inside the robot in the form of a list of data storage structures.

It is noted that a corresponding one of the candidate navigation routes within the candidate navigation network is allowed to traverse the obstacle grid point marked on the grid map, regardless of whether this obstacle grid point is marked for map errors.

In the present embodiment, the grid marking error is formed by reflecting an error such as a sensor error or a map drift error or an obstacle movement into the grid map.

Step S106, when the child node searched in the neighborhood is not the end point of all the candidate navigation routes in the candidate navigation network, setting the free grid point searched in the neighborhood in step S103 as a node to be traversed, optionally adding the node to be traversed into the cache space of the node to be traversed, recording the position information of the father node of the free grid point searched in the neighborhood in step S103, and then entering step S108. Therefore, the searching range of the child nodes in the neighborhood searching process is enlarged, the route starting and stopping points in the candidate navigation network can be searched in a more extensive range, and the planned route has better practicability and representativeness.

As an embodiment of step S105, when the child node searched in the neighborhood in step S103 is the start point of a corresponding candidate navigation route in the candidate navigation network, setting the end point of the corresponding candidate navigation route as a node to be traversed, and optionally adding the cache space of the node to be traversed, and recording the position information of the parent node of the end point of the corresponding candidate navigation route, so that each candidate navigation route uses the end point thereof as route identification information in the candidate navigation network; and simultaneously setting all intermediate nodes between two end points of a corresponding candidate navigation route and the starting point of the same candidate navigation route as traversed nodes. Along the path extending direction of the start point of the corresponding one of the candidate navigation routes to the end point thereof, the parent node of each node on the corresponding one of the candidate navigation routes is located at a node position adjacent to the node. Specifically, in the candidate navigation network, starting from the start point of the corresponding one of the candidate navigation routes, and from the first path extending direction of the start point of the one of the candidate navigation routes to the end point thereof, the position information of the parent node of each node of the one of the candidate navigation routes is sequentially recorded until the position information of the parent node of the end point of the one of the candidate navigation routes is recorded, wherein the parent node of each node of the one of the candidate navigation routes is an adjacent node thereof in the first path extending direction of the one of the candidate navigation routes. Therefore, the parent node of the destination of the candidate navigation route is located outside of and adjacent to the destination of the candidate navigation route, and the child node of the destination of the candidate navigation route is located on and adjacent to the destination of the candidate navigation route; the parent node of the start point of the candidate navigation route is located on the belonging candidate navigation route and adjacent to the start point of the candidate navigation route, and the child node of the start point of the candidate navigation route is located on the belonging candidate navigation route and adjacent to the start point of the candidate navigation route.

As another embodiment of step S105, when the child node searched in the neighborhood of step S103 is the end point of the corresponding one of the candidate navigation routes in the candidate navigation network, setting the start point of the corresponding one of the candidate navigation routes as a node to be traversed, and optionally adding the cache space of the node to be traversed, and recording the position information of the parent node of the start point of the corresponding one of the candidate navigation routes, so that each of the candidate navigation routes uses the start point thereof as route identification information in the candidate navigation network; and simultaneously setting all intermediate nodes between two end points of a corresponding candidate navigation route and the end point of the same candidate navigation route as traversed nodes. And the parent node of each node on the corresponding one of the candidate navigation routes is positioned at a node position adjacent to the node along the path extending direction of the starting point of the end point of the corresponding one of the candidate navigation routes. Specifically, within the candidate navigation network, from the end point of the one candidate navigation route, along the second path extending direction in which the end point of the corresponding one candidate navigation route points to the start point thereof, the position information of the parent node of each node of the one candidate navigation route is sequentially recorded until the position information of the parent node of the start point of the one candidate navigation route is recorded, the parent node of each node of the one candidate navigation route being its adjacent node in the second path extending direction of the belonging candidate navigation route, so that the parent node of the start point of the one candidate navigation route is located outside and adjacent to the start point of the belonging candidate navigation route, and the child node of the start point of the one candidate navigation route is located on the belonging candidate navigation route and adjacent to the start point of the one candidate navigation route.

In the two embodiments based on step S105, the starting point or the ending point of the candidate navigation route is configured to be capable of uniquely determining a candidate navigation route, serving as the identifier corresponding to the candidate navigation route and recording the corresponding parent node information, while the remaining nodes of the same candidate navigation route are configured as traversed nodes, so that the search amount in the path planning process is reduced, and the backtracking of the matched candidate navigation route is accelerated.

Step S108, selecting the node with the minimum path cost and value from the nodes to be traversed as the next father node, and then entering step S109. In this embodiment, by comparing path costs of all nodes to be traversed, a better node is selected as the next parent node; the node with the smaller path cost and value has higher priority, the node with the smallest path cost and value (highest priority) is taken as the next father node, new neighborhood searching is started, the next father node is taken as a searching center to search and new neighborhood grid points (which are different from the nodes existing in the traversed node cache space) are added.

In this embodiment, the path cost sum value is obtained by adding or weighting a traversed path cost and a predicted path cost, where the traversed path cost is a cost of a specific node from the navigation start point among all the nodes to be traversed or in the cache space of the nodes to be traversed, the predicted path cost is a cost of the same specific node from the navigation end point, and definitions of the path costs are heuristic functions derived from an a-algorithm. When the path cost sum value is smaller, configuring the traversing priority of the appointed node among all nodes to be traversed to be higher; the embodiment is based on an A heuristic function, and is expressed by using Manhattan distance, diagonal distance or Euclidean distance when calculating the movement cost between two adjacent nodes; if the robot is only allowed to move in the four directions of up, down, left and right in the map area, the Manhattan distance can be used; if Xu Chao eight directions are allowed to move in the map area where the robot is currently located, a diagonal distance may be used; the euclidean distance may be used if movement in any direction is allowed in the map area where the robot is currently located. The method is favorable for searching out the shortest path with the lowest navigation cost.

When calculating the path cost, starting from a navigation starting point, sequentially connecting each traversed grid center point until connecting to the designated node, acquiring the length of a connecting line formed by current connection according to the side length of the grid, and taking the length as the traversed path cost; when calculating the path cost, setting the designated node as a father node, acquiring all the connection schemes from each child node corresponding to the father node to the navigation terminal point, acquiring the length of each connection scheme corresponding to each child node according to the side length of the grid, and selecting the shortest length corresponding to each child node as the corresponding predicted path cost; wherein the grid points are represented by grid center points and are used for representing the position characteristics of a grid. The cost value calculated in this embodiment is a measure of the cost of the motion trajectory of the robot, and represents the cost of traveling from the start point to the designated node and then to the end point, including the path length.

It should be noted that, for each node, a cost value may be calculated, where the cost value is a measure of the cost of the motion track of the robot, and represents the cost of moving from the start point to the node and then moving to the end point, and includes factors such as path length, required time, whether collision occurs, whether the speed direction is frequently switched, and so on.

Step S109, judging whether the next parent node is the navigation destination, if yes, proceeding to step S110, otherwise proceeding to step S111.

Step S111, updating the current parent node in step S103 with the next parent node, and returning to step S103.

For the grid map, the implementation mode corresponding to the previous steps is to continuously expand the process of reaching the navigation end point from the navigation start point, and the expanded searched path nodes are connected with the candidate navigation route to form the finally planned navigation path. Therefore, step S111 is a triggering step belonging to the loop execution, and each time the step S103 is returned, the step is extended once, so that the robot starts from the current parent node, runs for a preset interval time, and the node which can be reached is a child node; in short, each extension represents a robot "step" corresponding to crossing a grid once in the map; it should be noted that, the preset interval time is a periodic time of each expansion, and represents a smaller time unit, for example, may be 5 seconds, 10 seconds, etc., and the shorter the preset interval time is, the finer the planned navigation path is, so the preset interval time may be determined according to the actual requirement.

In this embodiment, in the process of expanding by using the next parent node as the search center, if the current parent node is searched in the neighborhood as the child node of the next parent node, the current parent node is not required to be set as the node to be traversed, and the node to be traversed is added into the cache space of the node to be traversed.

Step S110, determining that a navigation destination has been searched (or expanded) in the foregoing neighborhood searching process, and starting from the navigation destination based on the location information of the parent node recorded in the foregoing step, sequentially connecting the child nodes and their parent nodes except for all the intermediate nodes and their corresponding one end points of the candidate navigation route in step S104 until connecting to the navigation start point, and planning a path from the navigation start point to the navigation destination.

Compared with the prior art, the embodiment controls the robot to perform neighborhood search by selecting the node with the minimum path cost and value from the navigation start point as the father node at the same position, and adds the free grid point which is not located on the candidate navigation route and one endpoint (the start point or the end point) with the identification meaning on the candidate navigation route obtained by neighborhood search into the node cache space to be traversed, records the corresponding father node as the path node backtracking information, simultaneously eliminates the interference caused by repeated search of the intermediate node (although the intermediate node is suitable for the navigation target point of the narrow areas) on the candidate navigation route until the father node is the navigation end point, and reversely connects the candidate navigation route which is searched by connection to the navigation start point from the navigation end point according to the position relation of the father node and the father node, thereby fusing the candidate navigation route which accords with the passing condition into a heuristic search algorithm to plan a navigation route, overcoming the narrow channel in the grid and easily generating the marking error, enabling the robot to effectively enter and exit the narrow area along the planned whole navigation route, and reducing the probability of path failure.

Specifically, when it is determined that the next parent node is the navigation destination, the specific steps of sequentially connecting the child nodes and their parent nodes, except for all intermediate nodes and their corresponding one end points of the candidate navigation route in step 4, from the navigation destination based on the position information of the parent node recorded previously until the child nodes and their parent nodes are connected to the navigation destination include:

step 51, starting from the navigation terminal point, connecting the navigation terminal point and the parent node thereof based on the recorded position information of the parent node or the position information of the parent node and the child node stored in the node cache space to be traversed; step 52 is then entered. Thereby starting to make the reverse connection node by node.

Step 52, using the parent node determined currently as a child node, namely updating the parent node determined in step 51 as a child node, including location information; based on the recorded position information of the father node, connecting the father node of the child node; step 53 is then entered.

Step 53, repeating step 52 until connecting to step S105 to determine one end point of the searched corresponding one candidate navigation route, then starting from the other end point of the candidate navigation route, connecting the other end point of the corresponding one candidate navigation route and its parent node, and then returning to step 52. Wherein the parent node of the other end point of the corresponding one candidate navigation route is: step S105 determines a node adjacent to the other end point of the corresponding one of the candidate navigation routes and located outside the one of the candidate navigation routes in the path extending direction in which the searched end point points to the other end point of the one of the candidate navigation routes.

As an embodiment, when step 53 is connected to step S105 to determine the start point of the searched corresponding one of the candidate navigation routes, it is known from the foregoing embodiment that the start point of the corresponding one of the candidate navigation routes is the parent node of the child node updated in step 52, that is, the start point of the corresponding one of the candidate navigation routes becomes the parent node of the node adjacent thereto in the opposite direction to the path extending direction in which the start point of the corresponding one of the candidate navigation routes points to the end point thereof, whereby the path extending direction along which the start point of the corresponding one of the candidate navigation routes points to the end point thereof can be determined, the parent node of each of the nodes on the corresponding one of the candidate navigation routes being located at the node position adjacent to the node, while determining that the start point and the intermediate node of the corresponding one of the candidate navigation routes both belong to non-repeatable search nodes, that is, the traversed path nodes. Step 53 begins with the destination of the candidate navigational route and connects the destination of the candidate navigational route with its parent node because the parent node of the destination of the candidate navigational route is outside the candidate navigational route so that the return to step 52 continues back to the outside area of the candidate navigational route.

As another embodiment, when step 53 is connected to step S105 to determine the end point of the searched corresponding one candidate navigation route, it is known from the foregoing embodiment that the end point of the corresponding one candidate navigation route is the parent node of the child node updated in step 52, that is, the end point of the corresponding one candidate navigation route becomes the parent node of the node adjacent thereto in the path extending direction in which the start point of the corresponding one candidate navigation route points to the end point thereof, whereby it is determined that the parent node of each node on the corresponding one candidate navigation route is located at the node position adjacent to the node along the end point of the corresponding one candidate navigation route, and it is determined that both the start point and the intermediate node of the corresponding one candidate navigation route belong to the non-repeatedly searched node, that is, the traversed path node. Step 53 begins with the start of the candidate navigational route connecting the start of the candidate navigational route with its parent node because the parent node of the start of the candidate navigational route is outside the candidate navigational route so that the return to step 52 continues back to the outside area of the candidate navigational route.

And 54, repeating the step 52 and the step 53 until the parent nodes are connected to the navigation starting point, and realizing that the parent nodes searched in the steps 101 to 111 are reversely connected one by one based on the recorded position information of the parent nodes so as to obtain a navigation path from the navigation starting point to the navigation ending point. In the process of performing steps 51 to 54, the process of sequentially connecting the parent node of the child node (including the end point of the candidate navigation route) and taking the parent node as the child node may be referred to as "reverse expansion", which is essentially a path node backtracking process starting from the navigation end point, continuously expanding and connecting to the start point and the stop point of the candidate navigation route until reaching the navigation start point, wherein the track of the reverse expansion is: and obtaining a navigation path which is connected with the navigation starting point, can traverse the candidate navigation route of the barrier grid point and the navigation ending point.

Compared with the prior art, after the navigation path is planned through the steps, the embodiment realizes: when a robot searches a path through a narrow area by using a heuristic search algorithm or an incremental heuristic search algorithm, if the narrow area is detected, switching a navigation path searched by entering the heuristic search algorithm or the incremental heuristic search algorithm into the candidate navigation route near the narrow area; if the situation that the navigation path leaves the narrow area is detected, switching the candidate navigation path into a navigation path searched by a heuristic search algorithm or an incremental heuristic search algorithm; the robot can effectively pass through the narrow area, and the navigation success rate of the narrow area is improved.

Preferably, the state parameter of each node may also be marked into the navigation path, wherein the state parameter of each node comprises the moment of each node, i.e. the moment when the robot moves to each node according to the plan. Therefore, the state parameters can be added into the navigation path, for example, the time of each node is marked into the navigation path, and when the narrow area exists in the map, whether the robot is trapped in the narrow area for a long time can be calculated, so that effective path planning is realized in a complex obstacle layout scene, and the actual requirements are met.

On the basis of the foregoing embodiment, if the robot detects that the narrow area is changed to not be located in the narrow area, entering the new area may be regarded as the robot getting rid of the trouble, the path planning method starts to be executed, i.e. the steps S101 to S111 are executed iteratively, so as to plan a navigation path supporting the robot to freely enter and exit the narrow area, and overcome the map drift error. Therefore, when the robot leaves the narrow area or just before leaving the current narrow area and entering a new narrow area, the searched coordinate points which are suitable for the candidate navigation route passing through the narrow area are fused into the current position to program a navigation route by using a heuristic search algorithm instead of performing navigation route planning by means of neighborhood search, and the problem that the planned route cannot pass through the narrow area due to the fact that free grid points searched at the narrow area are easily marked as barrier grid points is solved.

Preferably, the interval between the non-narrow area (including the open area and the obstacle area) and the narrow area currently traversed by the robot is set to a preset distance as a basis for determining whether the robot enters the new area, the preset distance may be 60 cm, if the preset distance is greater than or equal to 60 cm, the robot enters a new area and leaves the narrow area, and the situation may also be regarded as getting rid of poverty.

It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.

The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention in any way, and any person skilled in the art may make modifications or alterations to the above disclosed technical content to the equivalent embodiments. However, any simple modification, equivalent variation and variation of the above embodiments according to the technical substance of the present invention still fall within the protection scope of the technical solution of the present invention.

Claims

1. A method of route optimization, comprising:

firstly, controlling a robot to move along a preset path and planning at least one local route; the preset path is a preset path for supporting the robot to pass through a narrow area;

Then, randomly searching a first preset number of grid points meeting first communication screening conditions in the grid map constructed by the robot;

respectively taking the grid points meeting the first communication screening condition, the starting points of all local routes and the end points of the local routes as a first search center, searching out grid points meeting the second communication screening condition, connecting the first search center and all the searched grid points meeting the second communication screening condition into a first feasible route, and further determining that the first feasible route is connected with the local routes;

when grid points meeting the first communication screening condition exist in all the child nodes of the starting points of all the local routes and all the child nodes of the ending points of all the local routes, determining that a first feasible route is connected with the local routes to form a complete candidate navigation network;

when the grid points meeting the first communication screening condition do not exist in all the child nodes of the starting points of all the local routes and all the child nodes of the ending points of all the local routes, respectively taking the starting points of all the local routes and the ending points of all the local routes as second search centers, searching out grid points meeting the third communication screening condition, connecting the second search centers and all the searched grid points meeting the third communication screening condition into a second feasible route, and further determining that the first feasible route and the second feasible route are connected with the local route to form a complete candidate navigation network;

The first communication screening condition is: the free grid points searched at present do not exist in the feasible route node set, and no barrier grid points exist in the area with the free grid points searched at present as circle centers and the searching radius as one body diameter, and no unknown grid points exist, so that the free grid points searched at present are first communication nodes;

the second connectivity screening conditions include: the grid points which do not have barrier grid points on the connecting line with the first search center and the connecting line length with the first search center is smaller than a preset distance threshold value are first communication nodes or second communication nodes; the preset distance threshold is preset according to specific environmental conditions; the preset distance threshold is larger than the side length of the single grid;

the third connectivity screening condition includes: among all grid points which have no obstacle grid points and no unknown grid points on the connection line with the second search center, the grid point with the shortest connection line length with the second search center is a first connection node.

2. The route optimization method according to claim 1, wherein the method for randomly searching the first preset number of grid points meeting the first connection screening condition in the grid map constructed by the robot specifically comprises:

Calculating a product based on the ratio of the size of the grid map constructed by the robot to the side length of the single grid, obtaining the total number of all grids distributed in the grid map constructed by the robot, and determining the total number as the first preset number; wherein, the grid points are represented by grid center points corresponding to one grid;

randomly searching grid points meeting the first communication screening condition in the grid map by taking the current position of the robot as a searching center, and adding a pre-established feasible route set until randomly searching and obtaining the first preset number of grid points meeting the first communication screening condition;

wherein, set up the grid point that satisfies the first communication screening condition as the first communication node.

3. The route optimization method according to claim 1, characterized in that the route optimization method further comprises:

setting the starting point and the ending point of the local route as second communication nodes; meanwhile, on the local route, setting an intermediate node between a starting point and an ending point of the local route as a third communication node;

recording the position information of the child node of the second communication node in the predicted passing node set, and adding the second communication node into the feasible route node set; recording the position information of the child node of the third communication node in the same predicted passing node set, and adding the third communication node into the feasible route node set; so that the feasible route node set stores one complete path node which is used for representing a corresponding unique local route in the prediction passing node set; the predicted passing node set is created in the process that the robot moves along the preset path;

The second communication node and the child nodes thereof are adjacent and communicated; the third connected node and its child nodes are adjacent and connected.

4. The route optimization method of claim 3, further comprising: when the second communication node and the third communication node join the feasible route node set, the second communication node and the third communication node in the feasible route node set are sequentially connected into the local route according to the adjacent relation of the child node and the father node thereof based on the recorded position information of the child node.

5. The route optimization method according to claim 2, wherein the method for searching the grid points meeting the second communication screening condition by using the grid points meeting the first communication screening condition, the starting points of all local routes and the end points thereof as the first search center, and connecting the first search center and all the searched grid points meeting the second communication screening condition into the first feasible route specifically comprises:

the first communication node and the second communication node are respectively used as the first search center, grid points meeting second communication screening conditions are searched in the feasible route node sets, the grid points meeting the second communication screening conditions are set as first legal sub-nodes of the corresponding first search centers and added into the sub-node sets of the corresponding first search centers, wherein the sub-node sets of the corresponding first search centers also exist in the feasible route node sets, and each first communication node and each second communication node have a corresponding sub-node set in the feasible route node sets;

And then connecting the same first search center and all first legal sub-nodes to form the first feasible route, and further determining that the first feasible route is connected with the local route.

6. The route optimization method according to claim 5, wherein the method for determining that the first feasible route is connected with the local route to form a complete candidate navigation network when the grid points satisfying the first communication screening condition exist in both the child nodes of the start points of all the local routes and the child nodes of the end points of all the local routes comprises:

if the fact that all the sub-nodes of the second communication nodes in the feasible route node set are all provided with the first communication nodes is detected, determining that two ends of a local route to which all the second communication nodes belong are respectively connected with all the first communication nodes in the sub-node set added into the corresponding first search center, and further determining that the local route and the first feasible route are connected to form a complete candidate navigation network;

the currently detected child nodes of the second communication node are in the child node set corresponding to the same second communication node.

7. The route optimization method according to claim 6, wherein when the grid points satisfying the first communication screening condition do not exist in the child nodes of the start points of all the local routes and the child nodes of the end points of all the local routes, searching out the grid points satisfying the third communication screening condition by using the start points of all the local routes and the end points thereof as the second search centers, and connecting the second search centers and all the searched grid points satisfying the third communication screening condition into the second feasible route, the method comprises:

When detecting that all the sub-nodes of the second communication nodes in the feasible route node set do not have the first communication nodes, respectively taking each second communication node in the feasible route node set as the second search center, searching out grid points meeting third communication screening conditions in the feasible route node set, setting the grid points meeting the third communication screening conditions as third legal sub-nodes of the corresponding second search center, adding the third legal sub-nodes into the sub-node set of the corresponding second search center, increasing elements of the sub-node set corresponding to each second communication node, connecting the same second search center with all the third legal sub-nodes as the second feasible route, and further determining that the local route is connected with the second feasible route;

8. Route optimization method according to any of the claims 1 to 7, characterized in that in the complete candidate navigation network both ends of the local route are used for connecting an end point of the first feasible route and/or an end point of the second feasible route, respectively, but neither the first feasible route nor the second feasible route is connected to an intermediate node of the local route; so as to search a route from a preset navigation starting point to a preset navigation ending point in the complete candidate navigation network by using a path search algorithm;

The narrow area is a crack channel of two or more obstacles, the crack channel is a gap corresponding to the narrowest part between the two obstacles, and the width of the gap is larger than or equal to the diameter of the robot body;

wherein the node set is a data structure occupying memory space.

9. The route optimization method according to claim 8, wherein the local route planning method includes:

step S1, controlling the robot to move along a preset path until judging that the preset search area meets a first preset round-area passing condition, and then entering step S2;

s2, recording the current position of the robot as a first path node, simultaneously creating a new predicted passing node set, adding the first path node into the predicted passing node set, and then entering step S3;

step S3, recording the current position of the robot as a second path node every time the robot moves a straight line distance of a body diameter along the preset path, and then entering step S4;

step S4, when judging that the second path node meets a second preset round domain passing condition, adding the second path node into the predicted passing node set in the step S2;

The path nodes stored in the predicted passing node sets are connected into a corresponding local route according to the sequence of joining, so that one predicted passing node set correspondingly represents a unique local route.

10. The route optimization method according to claim 9, wherein when it is determined that the second path node does not satisfy the second preset round-domain traffic condition, further comprising:

step S6, according to the number of path nodes stored in the predicted passing node set, storing the predicted passing node set into a local route node set, and returning to the step S1;

wherein, one local route node set has at least one predicted passing node set; within the same local route node set, the head element and the tail element of any one of the prediction passing node sets are unique, and the head element and the tail element of the same prediction passing node set are not identical.

11. The route optimization method according to claim 10, wherein the method for storing the predicted passing node set into the same local route node set according to the number of path nodes stored in the predicted passing node set, and then returning to step S1 specifically comprises:

When the number of path nodes stored in the predicted passing node set is smaller than 2, deleting the predicted passing node set, and returning to the step S1 to create a new predicted passing node set;

when the number of path nodes stored in the predicted passing node set is greater than or equal to 2, the predicted passing node set is used for independently representing a local route and is stored in the local route node set so as to be called by a path searching algorithm, and then the step S1 is returned to create a new predicted passing node set;

the path node corresponding to the first element in each predicted passing node set is the starting point of a corresponding local route, and the path node corresponding to the tail element in each predicted passing node set is the ending point of a corresponding local route.

12. The route optimization method according to claim 11, wherein the first preset circle field traffic condition includes:

the ratio of the area of the first non-passable area to the area of the pre-search area is greater than the first traffic evaluation value;

the first non-passable area is a grid area consisting of unknown grid points and barrier grid points in the grid area corresponding to the first circular area; the first traffic evaluation value is a pre-judgment threshold value set for overcoming a marking error of a free grid point existing in the construction grid map;

The pre-search area is a first circular area taking the current position of the robot as a circle center and the diameter of the robot body as a radius.

13. The route optimization method according to claim 9, wherein the second preset circle field traffic condition includes:

in a second circular area taking the current position of the robot as the center of a circle and the diameter of the body of the robot as the radius, the ratio of the area of the second non-passable area to the area of the second circular area is larger than a second passable evaluation value; the second non-passable area is a grid area consisting of unknown grid points and barrier grid points in the grid area corresponding to the second circular area; the second pass evaluation value is a judgment threshold value set for overcoming a marking error of a free grid point existing in the construction grid map, and is larger than the first pass evaluation value.

14. The route optimization method according to claim 9, wherein the second preset circle field traffic condition includes:

searching a path of a second path node which is recorded latest to a first path node which is recorded latest in a second circular area which takes the first path node which is recorded latest as a circle center and takes the diameter of a machine body which is a preset multiple as a radius by using a path searching algorithm;

Wherein, the fuselage diameter of predetermineeing the multiple sets up to: the second circular areas are not allowed to intersect other marked grid areas.

15. The route optimization method according to claim 9, characterized in that in the course of executing said step S1, further comprising: firstly judging whether the robot moves to the end point of the preset path, if so, stopping executing the planning method of the local route, otherwise, controlling the robot to continuously move along the preset path, and judging whether the pre-search area meets a first preset circle domain passing condition;

in the process of executing step S3, further comprising: judging whether the robot moves to the end point of the preset path, if so, stopping executing the planning method of the local route, otherwise, controlling the robot to continue to move along the preset path.

16. A path planning method, characterized in that the path planning method comprises: performing the route optimization method of any one of claims 1 to 15;

after the route optimization method is performed, configuring the first feasible route, the second feasible route and the local route as candidate navigation routes in a candidate navigation network; further comprises:

Step 1, setting a navigation starting point and a navigation end point in a grid map;

step 2, setting a navigation starting point as a current father node and as a node to be traversed;

step 3, performing neighborhood search in the grid map by taking the current father node as a search center, wherein 8 grid points adjacent to the current father node are respectively used as child nodes of the current father node;

step 4, when the child node searched in the neighborhood in the step 3 is one end point of a corresponding candidate navigation route in the candidate navigation network searched in advance, setting the other end point of the corresponding candidate navigation route as a node to be traversed; setting all intermediate nodes and the end points between two end points of a corresponding candidate navigation route as traversed nodes, and setting free grid points which are not searched in the step 3 and are not on the corresponding candidate navigation route as nodes to be traversed; then enter step 5; wherein, all nodes set as to be traversed are correspondingly recorded with the position information of the father node for subsequent backtracking of the path node;

step 5, selecting a node with the minimum path cost and value from all nodes to be traversed as a next father node, judging whether the next father node is the navigation destination or not, if so, starting from the navigation destination based on the recorded position information of the father node, and sequentially connecting child nodes and father nodes except all intermediate nodes of the candidate navigation route and corresponding one end points in step 4 until the nodes are connected to the navigation start point, and planning a path from the navigation start point to the navigation destination; otherwise, the next father node is updated to the current father node in the step 3, and the step 3 is returned.

17. The path planning method of claim 16, wherein step 4 further comprises:

and 3, setting the free grid points searched in the neighborhood in the step 3 as nodes to be traversed when the child nodes searched in the neighborhood are not endpoints of all candidate navigation routes, recording the position information of the father nodes of the free grid points searched in the neighborhood in the step 3, and then entering the step 5.

18. The path planning method according to claim 17, wherein in the step 4, when the child node searched in the neighborhood of the step 3 is the start point of the corresponding one of the candidate navigation routes in the candidate navigation network, setting the end point of the corresponding one of the candidate navigation routes as a node to be traversed, and recording the position information of the parent node of the end point of the corresponding one of the candidate navigation routes so that each of the candidate navigation routes uses the end point thereof as route identification information in the candidate navigation network; setting all intermediate nodes between two endpoints of a corresponding candidate navigation route and the starting point of the same candidate navigation route as traversed nodes;

and the parent node of each node on the corresponding one of the candidate navigation routes is positioned at a node position adjacent to the node along the path extending direction of the starting point of the corresponding one of the candidate navigation routes to the ending point of the corresponding one of the candidate navigation routes.

19. The path planning method according to claim 17, wherein in the step 4, when the child node searched in the neighborhood of the step 3 is the end point of the corresponding one of the candidate navigation routes in the candidate navigation network, setting the start point of the corresponding one of the candidate navigation routes as a node to be traversed, and recording the position information of the parent node of the start point of the corresponding one of the candidate navigation routes so that each of the candidate navigation routes uses the start point thereof as route identification information in the candidate navigation network; setting all intermediate nodes between two endpoints of a corresponding candidate navigation route and the endpoint of the same candidate navigation route as traversed nodes;

and the parent node of each node on the corresponding one of the candidate navigation routes is positioned at a node position adjacent to the node along the path extending direction of the starting point of the end point of the corresponding one of the candidate navigation routes.

20. The path planning method according to claim 18, wherein in the step 5, the specific steps of sequentially connecting child nodes and their parent nodes, except for all intermediate nodes of the candidate navigation route and their corresponding one end points in the step 4, starting from the navigation end point based on the previously recorded position information of the parent node, until connecting to the navigation start point include:

Step 51, starting from the navigation destination point based on the recorded position information of the father node, and connecting the navigation destination point and the father node thereof; step 52 is then entered;

step 52, using the currently determined father node as the child node, and connecting the father node of the child node based on the recorded position information of the father node;

step 53, repeating step 52 until connecting to one end point of the corresponding one candidate navigation route in step 4, then starting from the other end point of the corresponding one candidate navigation route in step 4, connecting to the other end point of the corresponding one candidate navigation route in step 4 and its father node, and then returning to step 52;

step 54, repeating step 52 and step 53 until the navigation path is connected to the navigation starting point, and realizing reverse connection to obtain a navigation path from the navigation starting point to the navigation ending point.

21. The path planning method according to claim 20, wherein in the step 5, the path cost sum value is obtained by adding or weighting a traversed path cost and a predicted path cost, wherein the traversed path cost is a cost of a specified node from the navigation start point among all nodes to be traversed, and the predicted path cost is a cost of the same specified node from the navigation end point; when the path cost sum value is smaller, configuring the traversing priority of the appointed node among all nodes to be traversed to be higher;

Wherein the movement cost between two adjacent nodes is represented using a manhattan distance, a diagonal distance, or a euclidean distance.

22. The path planning method according to claim 21, wherein starting from a navigation start point, each traversed grid center point is sequentially connected until the traversed grid center point is connected to the designated node, and the length of a connecting line formed by current connection is obtained according to the side length of the grid and is used as the traversed path cost;

setting the designated node as a father node, acquiring all the connection schemes from each child node corresponding to the father node to a navigation terminal point, acquiring the length of each connection scheme corresponding to each child node according to the side length of the grid, and selecting the shortest length corresponding to each child node as the corresponding predicted path cost;

wherein the grid points are represented by grid center points and are used for representing the position characteristics of one grid.

23. A path planning method according to claim 16, wherein setting the traversed node does not allow the same node to be configured as the node to be traversed.

24. The path planning method of claim 23, wherein the step 4 further comprises: and if the free grid points which are not positioned on the corresponding candidate navigation route in the neighbor of the current father node or all the free grid points in the neighbor of the current father node are configured as the nodes to be traversed, changing the current father node from the nodes to be traversed to the traversed nodes.

25. A chip for storing program code for performing a route optimization method according to any one of claims 1 to 15 and/or for performing a path planning method according to claim 16.

26. A robot incorporating a chip according to claim 25 for performing a route optimisation method according to any one of claims 1 to 15 and/or a path planning method according to claim 16.