CN112985397B - Robot track planning method and device, storage medium and electronic equipment - Google Patents
Robot track planning method and device, storage medium and electronic equipment Download PDFInfo
- Publication number
- CN112985397B CN112985397B CN201911285207.7A CN201911285207A CN112985397B CN 112985397 B CN112985397 B CN 112985397B CN 201911285207 A CN201911285207 A CN 201911285207A CN 112985397 B CN112985397 B CN 112985397B
- Authority
- CN
- China
- Prior art keywords
- node
- child
- current
- cost value
- track
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 76
- 230000001351 cycling effect Effects 0.000 claims abstract description 9
- 238000010586 diagram Methods 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 8
- 238000011156 evaluation Methods 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 15
- 238000012545 processing Methods 0.000 description 10
- 230000006870 function Effects 0.000 description 7
- 238000007689 inspection Methods 0.000 description 6
- 230000003068 static effect Effects 0.000 description 5
- 238000004458 analytical method Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 230000001186 cumulative effect Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000012804 iterative process Methods 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
- 238000011144 upstream manufacturing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Business, Economics & Management (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Human Resources & Organizations (AREA)
- Automation & Control Theory (AREA)
- Strategic Management (AREA)
- Economics (AREA)
- Game Theory and Decision Science (AREA)
- Development Economics (AREA)
- Entrepreneurship & Innovation (AREA)
- Marketing (AREA)
- Operations Research (AREA)
- Quality & Reliability (AREA)
- Tourism & Hospitality (AREA)
- General Business, Economics & Management (AREA)
- Theoretical Computer Science (AREA)
- Manipulator (AREA)
Abstract
The embodiment of the invention relates to a robot track planning method, a device, a storage medium and electronic equipment, and relates to the technical field of robots, wherein the method comprises the following steps: expanding a plurality of child nodes corresponding to the first parent node in an open list; determining a first current child node corresponding to a first father node in each child node by taking the minimum total cost value as a principle; storing the first current child node into a target child node list, and replacing the preset cost value by utilizing the accumulated cost value of the first current child node; sequentially cycling the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the cycle number is larger than the preset number; and when the cycle times are not less than the preset times and the last current sub-node is not the termination node, planning the track of the robot according to each current sub-node in the target sub-node list. The embodiment of the invention improves the accuracy of the planned track.
Description
Technical Field
The embodiment of the invention relates to the technical field of robots, in particular to a robot track planning method, a robot track planning device, a computer readable storage medium and electronic equipment.
Background
In recent years, indoor positioning and navigation techniques have been rapidly developed. Indoor path planning is an important function in indoor navigation applications, and an optimal path between a starting point and a target position is planned for a user.
In the existing path planning methods of the indoor robots, most of the existing path planning methods generate corresponding paths through corresponding path generation algorithms, so that each indoor robot can travel to a target position according to the generated paths.
However, the above method has the following drawbacks: on the one hand, only path generation can be performed, and a track cannot be planned, so that the problem of mutual collision exists in the process that the robot runs to a target position according to the generated path; on the other hand, although the generated path may be added with a time stamp to form a track, the method may make the accuracy of the formed track low, and thus may cause a problem of robot deadlock.
Therefore, it is necessary to provide a new robot trajectory planning method and apparatus.
It should be noted that the information of the present invention in the above background section is only for enhancing the understanding of the background of the present invention and thus may include information that does not form the prior art that is already known to those of ordinary skill in the art.
Disclosure of Invention
The present invention aims to provide a robot trajectory planning method, a robot trajectory planning device, a computer-readable storage medium, and an electronic device, which further overcome, at least to some extent, the problem of low accuracy of the constructed trajectory due to the limitations and drawbacks of the related art.
According to one aspect of the present disclosure, there is provided a robot trajectory planning method including:
And a child node expansion step: the method comprises the steps that a starting node is used as a first father node to be placed in an open list formed by a plurality of three-dimensional grids in a three-dimensional grid graph, and a plurality of child nodes corresponding to the first father node are expanded in the open list; wherein one of the three-dimensional grids corresponds to one node;
The current child node determining step: the first father node is put into a closed list, and a first current child node corresponding to the first father node is determined in each child node by taking the minimum total cost value as a principle; wherein the total cost value includes an accumulated cost value and an estimated cost value;
the current child node storing step: when the accumulated cost value of the first current sub-node is determined to be smaller than a preset cost value, the first current sub-node is stored in a target sub-node list, and the preset cost value is replaced by the accumulated cost value of the first current sub-node;
The circulation steps are as follows: sequentially cycling the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the cycle number is larger than the preset number;
Track planning step: and when the cycle times are not less than the preset times and the last current sub-node is not the termination node, planning the track of the robot according to each current sub-node in the target sub-node list.
In an exemplary embodiment of the present disclosure, the trajectory planning step further includes:
And when the cycle times are not less than the preset times and the last current child node is a termination node, planning the track of the robot according to each father node in the closed list.
In an exemplary embodiment of the present disclosure, the robot trajectory planning method further includes:
Three-dimensional space construction: taking the length and the width of the obstacle included in the interval to be searched as a two-dimensional plane X axis and a Y axis, taking the height of the obstacle as a time T axis, and constructing the three-dimensional space according to the X axis, the Y axis and the T axis;
The three-dimensional grid diagram construction step: dividing the interval to be searched into a plurality of three-dimensional grids with the same volume based on the three-dimensional space, and obtaining the three-dimensional grid map according to each three-dimensional grid;
a starting node and a terminating node determining step: and determining a starting node and a terminating node according to the current position of the robot in the interval to be searched and the target position of the target object in the interval to be searched.
In one exemplary embodiment of the present disclosure, expanding a plurality of child nodes corresponding to the first parent node in the open list includes:
Expanding child nodes corresponding to the first father node according to a preset expansion rule according to the position of the first father node in the open list; wherein the number of the child nodes corresponding to the first parent node is not more than nine.
In an exemplary embodiment of the present disclosure, determining, in each of the child nodes, a first current child node corresponding to the first parent node on the basis of a total cost value minimum includes:
Calculating an accumulated cost value from the first parent node to each of the child nodes, and calculating an estimated cost value from each of the child nodes to the termination node;
And obtaining the total cost value of each child node according to each accumulated cost value and each estimated cost value, and taking the child node with the minimum total cost value as the first current child node.
In one exemplary embodiment of the present disclosure, calculating an estimated cost value from each of the child nodes to the termination node includes:
configuring weight parameters of each child node and each termination node on a time T axis;
And obtaining the estimated cost value according to the weight parameter, the child nodes and the Manhattan distance between the termination nodes.
In an exemplary embodiment of the present disclosure, after the trajectory planning step, the robot trajectory planning method further includes:
And (3) adaptability evaluation: and carrying out adaptability evaluation on the track according to the length of the track and the distance between the tail end position of the track and the termination node.
According to one aspect of the present disclosure, there is provided a robot trajectory planning device including:
the child node expansion module is used for placing the initial node serving as a first father node into an open list formed by a plurality of three-dimensional grids in the three-dimensional grid graph, and expanding a plurality of child nodes corresponding to the first father node in the open list; wherein one of the three-dimensional grids corresponds to one node;
The current child node determining module is used for placing the first father node into a closed list, and determining a first current child node corresponding to the first father node in each child node by taking the total cost value as the minimum principle; wherein the total cost value includes an accumulated cost value and an estimated cost value;
The current sub-node storage module is used for storing the first current sub-node into a target sub-node list when the accumulated cost of the first current sub-node is smaller than a preset cost, and replacing the preset cost by utilizing the accumulated cost of the first current sub-node;
the circulation module is used for sequentially circulating the operations executed by the child node expansion module, the current child node determination module and the current child node storage module and judging whether the circulation times are larger than preset times or not;
And the track planning module is used for planning the track of the robot according to each current sub-node in the target sub-node list when the cycle times are not less than the preset times and the last current sub-node is not the termination node.
According to one aspect of the present disclosure, there is provided a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the robot trajectory planning method of any one of the above.
According to one aspect of the present disclosure, there is provided an electronic device including:
A processor; and
A memory for storing executable instructions of the processor;
wherein the processor is configured to perform the robot trajectory planning method of any one of the above via execution of the executable instructions.
According to the robot trajectory planning method provided by the embodiment of the invention, on one hand, a starting node is taken as a first father node to be placed in an open list formed by a plurality of three-dimensional grids in the three-dimensional grid graph, and a plurality of child nodes corresponding to the first father node are expanded in the open list; then the first father node is put into a closed list, and a first current child node corresponding to the first father node is determined in each child node by taking the minimum total cost value as a principle; when the accumulated cost value of the first current sub-node is smaller than the preset cost value, the first current sub-node is stored in a target sub-node list, and the preset cost value is replaced by the accumulated cost value of the first current sub-node; then sequentially circulating the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the circulating times are larger than preset times or not; finally, when the cycle times are judged to be not less than the preset times and the last current sub-node is not the termination node, planning the track of the robot according to each current sub-node in the target sub-node list, so that the problem that the robots collide with each other in the process of traveling to the target position according to the generated path because the track cannot be planned due to the fact that only the path generation can be performed in the prior art is solved, and the working efficiency of the robots is improved; on the other hand, the problem that in the prior art, although the time stamp can be added on the generated path to form the track, the accuracy of the formed track is low, so that the robot is deadlocked is solved, and the accuracy of the planned track is improved; on the other hand, the concept of three-dimensional grids is introduced on the basis of the planar track, so that the planned track can not only consider the obstacle on the plane, but also consider the obstacle on the space, the track planning of the robot in the whole space is realized, and the accuracy of the planned track is further improved; furthermore, the method can avoid the problem that the track cannot be planned due to the failure of searching the current child node corresponding to the father node, and when the searching of the current child node fails, a track which is as close to the termination node as possible can be generated according to each current child node in the child node list, so that the robot can move according to the track.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the invention as claimed.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and together with the description, serve to explain the principles of the invention. It is evident that the drawings in the following description are only some embodiments of the present invention and that other drawings may be obtained from these drawings without inventive effort for a person of ordinary skill in the art.
Fig. 1 schematically shows a flow chart of a robot trajectory planning method according to an exemplary embodiment of the invention.
Fig. 2 schematically shows a flow chart of another robot trajectory planning method according to an exemplary embodiment of the invention.
Fig. 3 schematically illustrates an exemplary diagram of a three-dimensional space constructed according to the length, width and height of obstacles included in a section to be searched according to an exemplary embodiment of the present invention.
Fig. 4 schematically illustrates an example diagram of a three-dimensional grid according to an example embodiment of the invention.
FIG. 5 schematically illustrates an exemplary diagram of expanding a parent node to obtain a plurality of child nodes, according to an exemplary embodiment of the present invention.
Fig. 6 schematically illustrates a flowchart of a method for determining a first current child node corresponding to the first parent node in each child node on the basis of a minimum total cost value, according to an exemplary embodiment of the invention.
Fig. 7 schematically shows a block diagram of a robot trajectory planning device according to an exemplary embodiment of the invention.
Fig. 8 schematically shows an electronic device for implementing the above-described robot trajectory planning method according to an exemplary embodiment of the present invention.
Detailed Description
Example embodiments will now be described more fully with reference to the accompanying drawings. However, the exemplary embodiments may be embodied in many forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of the example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to give a thorough understanding of embodiments of the invention. One skilled in the relevant art will recognize, however, that the invention may be practiced without one or more of the specific details, or with other methods, components, devices, steps, etc. In other instances, well-known aspects have not been shown or described in detail to avoid obscuring aspects of the invention.
Furthermore, the drawings are merely schematic illustrations of the present invention and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus a repetitive description thereof will be omitted. Some of the block diagrams shown in the figures are functional entities and do not necessarily correspond to physically or logically separate entities. These functional entities may be implemented in software or in one or more hardware modules or integrated circuits or in different networks and/or processor devices and/or microcontroller devices.
The research and development of indoor inspection robots are important businesses researched by various enterprises in recent years, and the inspection robots are more similar to robots than outdoor unmanned vehicles, so the inspection robots are different from unmanned vehicles from chassis models to actual sizes.
The common indoor robot path planning related algorithm is A, and the indoor inspection robot can directly move to all directions without worrying about the Ackerman corner factor, so that the kinematic model is relatively simple. However, the algorithm a can only generate a path, but cannot plan a track, and although a timestamp can be additionally matched on the basis of the path a to form the track, the optimality of the track planning task is lost, and vehicle deadlock is sometimes caused. Therefore, development of a track planning method for an indoor robot, which can realize track planning at one time, is urgently needed.
In addition, it is desirable that the track planning process not fail easily, and even if there is no trafficable track scheme on the road, the algorithm should generate a track that tends to the end point as much as possible, instead of simply issuing a failure. However, at present, no track planning method for the indoor robot capable of realizing the functions exists.
In this example embodiment, a method for planning a robot trajectory is provided first, where the method may run on a server, a server cluster, a cloud server, or the like, or may run on a terminal device; of course, those skilled in the art may also operate the method of the present invention on other platforms as required, and this is not a particular limitation in the present exemplary embodiment. Referring to fig. 1, the robot trajectory planning method may include the steps of:
S110, child node expansion: the method comprises the steps that a starting node is used as a first father node to be placed in an open list formed by a plurality of three-dimensional grids in a three-dimensional grid graph, and a plurality of child nodes corresponding to the first father node are expanded in the open list; wherein one of the three-dimensional grids corresponds to one node.
Step S120, determining a current child node: the first father node is put into a closed list, and a first current child node corresponding to the first father node is determined in each child node by taking the minimum total cost value as a principle; wherein the total cost value includes an accumulated cost value and an estimated cost value.
S130, storing the current child node: when the accumulated cost value of the first current sub-node is determined to be smaller than a preset cost value, the first current sub-node is stored in a target sub-node list, and the preset cost value is replaced by the accumulated cost value of the first current sub-node.
And S140, sequentially cycling the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the cycling times are larger than preset times.
S150, track planning: and when the cycle times are not less than the preset times and the last current sub-node is not the termination node, planning the track of the robot according to each current sub-node in the target sub-node list.
In the robot trajectory planning method, on one hand, the initial node is taken as a first father node to be put into an open list formed by a plurality of three-dimensional grids in the three-dimensional grid graph, and a plurality of child nodes corresponding to the first father node are expanded in the open list; then the first father node is put into a closed list, and a first current child node corresponding to the first father node is determined in each child node by taking the minimum total cost value as a principle; when the accumulated cost value of the first current sub-node is smaller than the preset cost value, the first current sub-node is stored in a target sub-node list, and the preset cost value is replaced by the accumulated cost value of the first current sub-node; then sequentially circulating the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the circulating times are larger than preset times or not; finally, when the cycle times are judged to be not less than the preset times and the last current sub-node is not the termination node, planning the track of the robot according to each current sub-node in the target sub-node list, so that the problem that the robots collide with each other in the process of traveling to the target position according to the generated path because the track cannot be planned due to the fact that only the path generation can be performed in the prior art is solved, and the working efficiency of the robots is improved; on the other hand, the problem that in the prior art, although the time stamp can be added on the generated path to form the track, the accuracy of the formed track is low, so that the robot is deadlocked is solved, and the accuracy of the planned track is improved; on the other hand, the concept of three-dimensional grids is introduced on the basis of the planar track, so that the planned track can not only consider the obstacle on the plane, but also consider the obstacle on the space, the track planning of the robot in the whole space is realized, and the accuracy of the planned track is further improved; furthermore, the method can avoid the problem that the track cannot be planned due to the failure of searching the current child node corresponding to the father node, and when the searching of the current child node fails, a track which is as close to the termination node as possible can be generated according to each current child node in the child node list, so that the robot can move according to the track.
Hereinafter, each step involved in the robot trajectory planning method according to the exemplary embodiment of the present invention will be explained and illustrated in detail with reference to the accompanying drawings.
First, the most primitive a search algorithm on the X-Y two-dimensional plane will be briefly described. Before introducing an original fault-tolerant high-dimensional grid A-search method, firstly, how to use an A-algorithm to realize the most original function, namely path planning, is introduced. Paths have fewer time dimensions than tracks, so tracks are significantly different from paths.
The algorithm a is a method of searching in the connected graph. The algorithm requires dividing a two-dimensional plane into grids, then infinite points in continuous plane space can be represented by a finite number of grids. All grids can be divided into unoccupied grids and occupied grids by judging whether each obstacle overlaps with those grids one by one. To this end, the two-dimensional plane may be described as a graph structure suitable for the a algorithm—a mesh graph, where the mesh points are called nodes. After determining the nodes where the starting point and the ending point of the path planning task are located, the algorithm A obtains a node sequence connecting the starting point and the ending point through searching.
Further, to facilitate a better explanation of the steps involved in fig. 1, the three-dimensional mesh, the start node, and the end node involved are explained and explained first.
Specifically, referring to fig. 2, the robot trajectory planning method may further include steps S210 to S230, which are described in detail below.
In step S210, a three-dimensional space construction step: taking the length and the width of the obstacle included in the interval to be searched as a two-dimensional plane X axis and a Y axis, taking the height of the obstacle as a time T axis, and constructing the three-dimensional space according to the X axis, the Y axis and the T axis.
In the present exemplary embodiment, first, the length and width of an obstacle included in a section to be searched are taken as two-dimensional plane X-axis and Y-axis; then, the height of the obstacle is taken as a time T-axis, and a three-dimensional space is constructed according to the X-axis, the Y-axis, and the T-axis. For example, to describe dynamic and static obstacles, the time axis may be considered as a motion state, so that moving obstacles are uniformly recorded in an X-Y-T three-dimensional state space, where the static obstacles correspond to upright columns having a footprint as a basic plane shape and a period of time interval length on the T axis as high, and the moving obstacles correspond to inclined columns, as shown in fig. 3.
In step S220, a three-dimensional mesh map construction step: dividing the interval to be searched into a plurality of three-dimensional grids with the same volume based on the three-dimensional space, and obtaining the three-dimensional grid map according to each three-dimensional grid.
In the present exemplary embodiment, first, the section to be searched is divided into a plurality of cubes (three-dimensional meshes) of the same volume along the directions of the X-axis, the Y-axis, and the T-axis. For example, if discretized along each dimension, the continuous X-Y-T three-dimensional space can be represented as a limited number of tiny cubes, each of which can act as a node. After determining the nodes occupied by the obstacles, a three-dimensional connected graph (three-dimensional grid graph) can be constructed.
In step S230, the start node and end node determining step: and determining the starting node and the ending node according to the current position of the robot in the interval to be searched and the target position of the target object in the interval to be searched.
Specifically, as shown with reference to fig. 4, 401 may be a start node, 402 may be a stop node, and 403 may be, for example, a blank node, 404 may be, for example, an obstacle occupying node. Further, when the start node and the end node are determined, an a-algorithm may be used to search the path in the graph, and the generated X-Y-T high-dimensional path is a rough track, which completely reflects the detour manner for all dynamic/static obstacles.
Hereinafter, the steps S110 to S150 in fig. 1 will be explained and explained.
In step S110, the child node expansion step: the method comprises the steps that a starting node is used as a first father node to be placed in an open list formed by a plurality of three-dimensional grids in a three-dimensional grid graph, and a plurality of child nodes corresponding to the first father node are expanded in the open list; wherein one of the three-dimensional grids corresponds to one node.
In the present exemplary embodiment, first, a start node is put as a first parent node into an Open List (Open List) made up of a plurality of three-dimensional meshes in a three-dimensional mesh map, and then a plurality of child nodes corresponding to the first parent node are expanded in the Open List. Specifically, according to the position of the first father node in the open list, expanding the child node corresponding to the first father node according to a preset expansion rule; wherein the number of the child nodes corresponding to the first parent node is not more than nine.
Specifically, referring to fig. 5, an Open List (Open List) may be, for example, as shown in a square in fig. 5; the first parent node (starting node) may be, for example, as shown at 401 in fig. 5. Further, the principle of the algorithm adopted in the present exemplary embodiment is: starting expanding search from the starting node to the surrounding, continuously selecting the node which is most hopeful to approach the ending node, and continuing expanding until the node is expanded to the end point. With continued reference to fig. 5, the child node corresponding to the first parent node may be, for example, a corresponding node in a square around 401. The preset expansion rule may be, for example, that only monotonically uniform forward expansion is allowed on the time T axis, because time will not stop or flow backwards; and, when expanding the child node, allow to be static in place in the X-Y plane besides allowing to expand 8 connected domain, therefore each father node can expand 9 child nodes at most. By the method, the accuracy of the expanded child nodes can be improved on the basis of ensuring the number of the child nodes, and the accuracy of the finally planned path is further improved.
In step S120, the current child node determining step: the first father node is put into a closed list, and a first current child node corresponding to the first father node is determined in each child node by taking the minimum total cost value as a principle; wherein the total cost value includes an accumulated cost value and an estimated cost value.
In the present exemplary embodiment, first, a first parent node is put into a closed List (Close List), and then a first current child node corresponding to the first parent node is determined among the child nodes on the basis of the minimum total cost value. Specifically, referring to fig. 6, determining a first current child node corresponding to the first parent node in each child node on the basis of the minimum total cost value may include steps S610 to S620, which will be described in detail below.
In step S610, an accumulated cost value from the first parent node to each of the child nodes is calculated, and an estimated cost value from each of the child nodes to the termination node is calculated.
In the present exemplary embodiment, first, the accumulated cost value from the first parent node to the child node is calculated, which may specifically include: firstly, configuring weight parameters of each child node and the termination node on a time T axis; and secondly, obtaining the estimated cost value according to the weight parameters, the Manhattan distance among the child nodes and the termination nodes. Then, the estimated cost value between each child node and the termination node is calculated again.
Specifically, each Node k in the grid should record the following attributes: the index values of f index value Node k. F, g index value Node k. G, h index value Node k. H, index of father Node k. Parent_id, boolean state quantity Node k. Is_open showing whether the current Node is in OpenList set, and Boolean state quantity Node k. Is_closed showing whether the current Node has been closed. The f index reflects the total cost value of the node, and the calculation method can be shown as follows:
Nodek.f＝Nodek.g+Nodek.h； (1)
Wherein Node k. G represents the cumulative cost value of expanding from the starting Node to the current Node k, and Node k. H represents the estimated cost value of expanding from the current Node to the ending Node. Also, since the a algorithm maintains a dynamic set named OpenList (open list) that stores a subscript that prepares the Node to be explored, if Node k is saved in OpenList at some point, then state Node k is_open should be set to 1; openList the saved Node is culled OpenList once explored and Node k.is_opened＝0、Nodek is set to closed=1.
Further, the first requirement of the algorithm a is to refine the attribute information for the start Node start, including setting Node start. G=0, calculating Node start. H, calculating Node start.f＝Nodestart.g+Nodestart. H, setting Node start. Parent_id=null, setting Node start. Is_open=1, and setting Node k. Is_closed=0, where the h value is calculated according to manhattan distance. After Node start is added to the empty set OpenList to act as the first element, loop iteration is started.
The Node with the smallest f-value needs to be selected from the set OpenList at the beginning of each round of cycle (if multiple nodes get the smallest f-value at the same time, the Node in the latest join OpenList is typically selected), assuming that the selected Node is Node current. Then, node current is expanded, specifically, the nodes around Node current and adjacent 8 connected areas can be regarded as potential expandable child nodes. After the expansion of up to 9 child nodes is completed, they will be analyzed one by one. The analysis may be performed for the child Node child. First check Node child. Is_closed is 1: if yes, the node is explored, the node should be directly abandoned, and the next child node is analyzed; if not, further determine Node child. Is_open is 0: if 0, node child has never been explored, should trigger operation A; if not 0, operation B is triggered. Subsequently, the analysis processing for the current child node is completed. Node current is deleted from OpenList after analysis processing is performed for all child nodes, and Node current.is_opened＝0、Nodecurrent is_closed=1 is set for it, and the child Node is put into a closed list as a parent Node. The current iteration round ends up.
Hereinafter, the above operation a will be explained. The first step is to determine whether Node child is occupied by an obstacle: if yes, then set Node child. Is_closed=1 and end the operation; if not, further processing is required. Setting its parent Node as Node current, namely:
Nodechild.parent_id＝Nodecurrent； (2a)
Node child. G was calculated according to the following formula:
Nodechild.g＝Nodecurrent.g+Euclidean_dist(Nodecurrent,Nodechild)； (2b)
Wherein euclidean_dist (Node a,Nodeb) is a function for calculating the Euclidean distance between Node a and Node b, and function g should be supplemented with an appropriate penalty for immobility in place, as opposed to parent Node going forward and backward, encouraging the vehicle to drive to the endpoint as soon as possible.
Then, node child. H needs to be calculated and determined:
Nodechild.f＝Nodechild.g+Nodechild.h； (2c)
Next, it is determined whether Node child is a termination Node: if so, jumping out of the whole outer layer iteration loop, and successfully completing the searching process (the completed output path mode is described later); if not, add Node child to OpenList and set accordingly:
Nodechild.is_opened＝1； (2d)
And
Nodechild.is_closed＝0； (2e)
If a child Node child exists in OpenList, it indicates that it has previously set a parent Node and that the parent Node must not be Node current, at which point operation B should be performed. The main content of operation B is to determine whether resetting the parent Node of Node child to Node current can make the f-number of the current child Node smaller, and if so, resetting its parent Node to Node current. Specifically, assuming that the parent Node of the current child Node child is Node old_parent, there is
Nodechild.f＝Nodeold_parent.g+Euclidean_dist(Nodeold_parent,Nodechild)+Nodechild.h;(3a)
On the other hand, since Node child can be also discovered by Node current, the cost value of Node child can be estimated again under the assumption that the parent Node of child Node child is Node current, and this alternative cost value is denoted as f *:
Nodechild.f*＝Nodecurrent.g+Euclidean_dist(Nodecurrent,Nodechild)+Nodechild.h;(3b)
If Node child.f*<Nodechild. F indicates that Node current is more advantageous than Node old_parent, then Node child should reset the parent Node of Node child to Node current and update the g and f values of Node current accordingly. Conversely, if Node child.f*≥Nodechild. F, then the parent Node of Node child need not be changed.
Furthermore, because the X-Y-T search grid has two dimensions of length and time at the moment, a weight parameter w time2length >0 is introduced to convert the time dimension into the length dimension. Taking Node child as Node A and the termination Node as Node B as examples, there are:
nodeA.h＝|nodeA.x-nodeB.x|+|nodeA.y-nodeB.y|+wtime2length·|nodeA.t-nodeB.t|;(4)
In step S620, according to each of the accumulated cost values and each of the estimated cost values, a total cost value of each of the child nodes is obtained, and the child node with the smallest total cost value is used as the first current child node.
Specifically, after the accumulated cost value and the estimated cost value are obtained, a total cost value may be obtained according to a sum of the accumulated cost value and the estimated cost value, and a child node with the minimum total cost value may be used as the first current child node.
In step S130, the current child node storing step: when the accumulated cost value of the first current sub-node is determined to be smaller than a preset cost value, the first current sub-node is stored in a target sub-node list, and the preset cost value is replaced by the accumulated cost value of the first current sub-node.
In this example embodiment, after the first current child node determines, it may be determined whether the cumulative cost value of the first current child node is smaller than a preset cost value, where the preset cost value may be a value corresponding to a minimum cumulative cost value corresponding to each current child node; if yes, the first current child node is stored in a target child node list, and then the preset cost value is replaced by the accumulated cost value. It should be noted that, if the cycle is the first cycle, the total cost value of the first parent node may be taken as the preset cost value.
In step S140, the steps of: and sequentially cycling the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the cycling times are larger than preset times or not.
In this exemplary embodiment, the preset number of times may be obtained according to a historical operation number, for example, the preset number of times may be in the order of 100-1000, and if the preset number of times is smaller than the order of 100, the accuracy of each current sub-node is lower due to too few cycle times; if the number of times is larger than 1000, the generation efficiency of each current child node is lower due to the excessive number of times of circulation, and the planning efficiency of the track is lower.
To this end, a closed list (CloseList) is available that includes all parent nodes, and it should be added that since the time dimension of the terminating node corresponds to the maximum allowed movement time of the robot, the search can be exited immediately upon finding that a child node coincides with the terminating node in the X-Y dimension during the search. In addition, the best node up to now, namely the node with the minimum h index which has been explored, can be additionally and dynamically stored through the target node list in the searching process, and can be used for generating a fault-tolerant track for completing searching tasks as much as possible when searching fails.
In step S150, the trajectory planning step: and when the cycle times are not less than the preset times and the last current sub-node is not the termination node, planning the track of the robot according to each current sub-node in the target sub-node list.
In this example embodiment, when the number of cycles is not less than (greater than or equal to) the preset number of times, but the last current child node is not the termination node, the track of the robot may be planned according to each current child node in the target child node list, so that the problem that the track cannot be planned due to the search failure of the current child node corresponding to the parent node is avoided, and when the search failure of the current child node, a track as close to the termination node as possible may be generated according to each current child node in the child node list, so that the robot may move according to the track. It should be noted that, because each current child node in the target word node list corresponds to a parent node, the corresponding parent node can be found according to each current child node, and then a corresponding track can be obtained according to each parent node; in addition, in the calculation process, the time consumption of the storage of each current sub-node is calculated by taking microseconds as a unit, and the data volume of each current sub-node is small, so that the storage is very convenient, and the overall calculation speed is not influenced.
Further, when the cycle number is not less than the preset number and the last current child node is the termination node, planning the track of the robot according to each father node in the closed list. It should be noted that, if the preset times are not reached in the cyclic process, but the number of times is expanded to the termination node, the iterative process is finished in advance, and the trajectory of the robot is planned according to each father node in the closed list.
Further, in order to evaluate the track, the robot track planning method further includes: and (3) adaptability evaluation: and carrying out adaptability evaluation on the track according to the length of the track and the distance between the tail end position of the track and the termination node.
Specifically, the adaptability evaluation is performed on the generated track quality, and the adaptability cost index fitness consists of two parts of a task completion degree val completeness and a track quality val quality, and the following specific formula can be shown:
fitness＝valcompleteness+valquality； (5)
Further, the track quality is reflected by the track corresponding path length. The task completion reflects the difference between the searched coarse track end position (x end,yend) and the termination position specified in the task (x goal,ygoal):
Wherein w large >0 is a sufficiently large weight coefficient, and the value of the weight coefficient should ensure that the adaptive cost index value of any fault-tolerant track is lower than the adaptive cost value of any track successfully connected with the task endpoint.
Thus, a complete X-Y-T three-dimensional A trajectory planning algorithm with fault tolerance can be provided:
algorithm 1.X-Y-T three-dimensional a algorithm.
Input: parameters such as a grid graph, a starting node, a terminating node, max_cycle, w large、wtime2length and the like;
and (3) outputting: a vehicle track at least connected with the starting point, and a track adaptation degree fitness;
/>
/>
Therefore, whether the actual problem is solved or not can be known, the method can ensure that a section of track which is close to the end point as much as possible can be output, and if the end point is expanded in the searching process, the iterative process is finished in advance. If the trajectory of the splice end point is still not found when the maximum number of iterations arrives, the most promising trajectory is output, while also using the fitness index to quantify the quality of this output trajectory.
It should be further added that the above-mentioned robot trajectory planning method is located downstream of the sensing and positioning module and upstream of the control and tracking module in the entire inspection robot system. It receives the information of the moving and static obstacles around the current inspection robot in the environment obtained by the sensing module (the track of the moving object may be other AGVs with higher priority or simply be the predicted track), and it also receives the information of the current position of the vehicle issued by the positioning module. In addition, after the track is generated, the chassis will perform closed loop tracking on this track.
The embodiment of the invention also provides a robot track planning device. Referring to fig. 7, the robot trajectory planning device may include a child node expansion module 710, a current child node determination module 720, a current child node storage module 730, a circulation module 740, and a trajectory planning module. Wherein:
The child node expansion module 710 may be configured to put a starting node as a first parent node into an open list formed by a plurality of three-dimensional grids in the three-dimensional grid map, and expand a plurality of child nodes corresponding to the first parent node in the open list; wherein one of the three-dimensional grids corresponds to one node;
the current child node determining module 720 may be configured to put the first parent node into a closed list, and determine, in each of the child nodes, a first current child node corresponding to the first parent node based on a principle that a total cost value is minimum; wherein the total cost value includes an accumulated cost value and an estimated cost value;
The current sub-node storage module 730 may be configured to store the first current sub-node into a target sub-node list when determining that the accumulated cost value of the first current sub-node is less than a preset cost value, and replace the preset cost value with the accumulated cost value of the first current sub-node;
The circulation module 740 may be configured to circulate the operations performed by the child node expansion module, the current child node determination module, and the current child node storage module in sequence, and determine whether the circulation number is greater than a preset number;
the trajectory planning module 750 may be configured to plan the trajectory of the robot according to each current child node in the target child node list when the number of loops is not less than a preset number and the last current child node is not a termination node.
In one exemplary embodiment of the present disclosure, the trajectory planning module may be further configured to:
And when the cycle times are not less than the preset times and the last current child node is a termination node, planning the track of the robot according to each father node in the closed list.
In an exemplary embodiment of the present disclosure, the robot trajectory planning device further includes:
And a three-dimensional space construction module: taking the length and the width of the obstacle included in the interval to be searched as a two-dimensional plane X axis and a Y axis, taking the height of the obstacle as a time T axis, and constructing the three-dimensional space according to the X axis, the Y axis and the T axis;
The three-dimensional grid diagram construction module can be used for dividing the interval to be searched into a plurality of three-dimensional grids with the same volume based on the three-dimensional space, and obtaining the three-dimensional grid diagram according to each three-dimensional grid.
The starting node and the ending node determining module: and determining a starting node and a terminating node according to the current position of the robot in the interval to be searched and the target position of the target object in the interval to be searched.
In one exemplary embodiment of the present disclosure, expanding a plurality of child nodes corresponding to the first parent node in the open list includes:
Expanding child nodes corresponding to the first father node according to a preset expansion rule according to the position of the first father node in the open list; wherein the number of the child nodes corresponding to the first parent node is not more than nine.
In an exemplary embodiment of the present disclosure, determining, in each of the child nodes, a first current child node corresponding to the first parent node on the basis of a total cost value minimum includes:
Calculating an accumulated cost value from the first parent node to each of the child nodes, and calculating an estimated cost value from each of the child nodes to the termination node;
And obtaining the total cost value of each child node according to each accumulated cost value and each estimated cost value, and taking the child node with the minimum total cost value as the first current child node.
In one exemplary embodiment of the present disclosure, calculating an estimated cost value from each of the child nodes to the termination node includes:
configuring weight parameters of each child node and each termination node on a time T axis;
And obtaining the estimated cost value according to the weight parameter, the child nodes and the Manhattan distance between the termination nodes.
In an exemplary embodiment of the present disclosure, the robot trajectory planning device further includes:
and the adaptability evaluation module can be used for evaluating the adaptability of the track according to the length of the track and the distance between the tail end position of the track and the termination node.
The specific details of each module in the above-mentioned robot trajectory planning device are already described in detail in the corresponding robot trajectory planning method, so that they will not be described in detail here.
It should be noted that although in the above detailed description several modules or units of a device for action execution are mentioned, such a division is not mandatory. Indeed, the features and functions of two or more modules or units described above may be embodied in one module or unit in accordance with embodiments of the invention. Conversely, the features and functions of one module or unit described above may be further divided into a plurality of modules or units to be embodied.
Furthermore, although the steps of the methods of the present invention are depicted in the accompanying drawings in a particular order, this is not required to or suggested that the steps must be performed in this particular order or that all of the steps shown be performed in order to achieve desirable results. Additionally or alternatively, certain steps may be omitted, multiple steps combined into one step to perform, and/or one step decomposed into multiple steps to perform, etc.
In an exemplary embodiment of the present invention, an electronic device capable of implementing the above method is also provided.
Those skilled in the art will appreciate that the various aspects of the invention may be implemented as a system, method, or program product. Accordingly, aspects of the invention may be embodied in the following forms, namely: an entirely hardware embodiment, an entirely software embodiment (including firmware, micro-code, etc.) or an embodiment combining hardware and software aspects may be referred to herein as a "circuit," module "or" system.
An electronic device 800 according to such an embodiment of the invention is described below with reference to fig. 8. The electronic device 800 shown in fig. 8 is merely an example and should not be construed as limiting the functionality and scope of use of embodiments of the present invention.
As shown in fig. 8, the electronic device 800 is embodied in the form of a general purpose computing device. Components of electronic device 800 may include, but are not limited to: the at least one processing unit 810, the at least one storage unit 820, a bus 830 connecting the different system components (including the storage unit 820 and the processing unit 810), and a display unit 840.
Wherein the storage unit stores program code that is executable by the processing unit 810 such that the processing unit 810 performs steps according to various exemplary embodiments of the present invention described in the above section of the "exemplary method" of the present specification. For example, the processing unit 810 may perform step S110 as shown in fig. 1: and a child node expansion step: the method comprises the steps that a starting node is used as a first father node to be placed in an open list formed by a plurality of three-dimensional grids in a three-dimensional grid graph, and a plurality of child nodes corresponding to the first father node are expanded in the open list; one of the three-dimensional grids corresponds to one node; step S120: the current child node determining step: the first father node is put into a closed list, and a first current child node corresponding to the first father node is determined in each child node by taking the minimum total cost value as a principle; wherein the total cost value includes an accumulated cost value and an estimated cost value; step S130: the current child node storing step: when the accumulated cost value of the first current sub-node is determined to be smaller than a preset cost value, the first current sub-node is stored in a target sub-node list, and the preset cost value is replaced by the accumulated cost value of the first current sub-node; step S140: sequentially cycling the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the cycling times are larger than preset times or not; step S150: track planning step: and when the cycle times are not less than the preset times and the last current sub-node is not the termination node, planning the track of the robot according to each current sub-node in the target sub-node list.
The storage unit 820 may include readable media in the form of volatile storage units, such as Random Access Memory (RAM) 8201 and/or cache memory 8202, and may further include Read Only Memory (ROM) 8203.
Storage unit 820 may also include a program/utility 8204 having a set (at least one) of program modules 8205, such program modules 8205 including, but not limited to: an operating system, one or more application programs, other program modules, and program data, each or some combination of which may include an implementation of a network environment.
Bus 830 may be one or more of several types of bus structures including a memory unit bus or memory unit controller, a peripheral bus, an accelerated graphics port, a processing unit, or a local bus using any of a variety of bus architectures.
The electronic device 800 may also communicate with one or more external devices 900 (e.g., keyboard, pointing device, bluetooth device, etc.), one or more devices that enable a user to interact with the electronic device 800, and/or any device (e.g., router, modem, etc.) that enables the electronic device 800 to communicate with one or more other computing devices. Such communication may occur through an input/output (I/O) interface 850. Also, electronic device 800 may communicate with one or more networks such as a Local Area Network (LAN), a Wide Area Network (WAN), and/or a public network, such as the Internet, through network adapter 860. As shown, network adapter 860 communicates with other modules of electronic device 800 over bus 830. It should be appreciated that although not shown, other hardware and/or software modules may be used in connection with electronic device 800, including, but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, data backup storage systems, and the like.
From the above description of embodiments, those skilled in the art will readily appreciate that the example embodiments described herein may be implemented in software, or may be implemented in software in combination with the necessary hardware. Thus, the technical solution according to the embodiments of the present invention may be embodied in the form of a software product, which may be stored in a non-volatile storage medium (may be a CD-ROM, a U-disk, a mobile hard disk, etc.) or on a network, and includes several instructions to cause a computing device (may be a personal computer, a server, a terminal device, or a network device, etc.) to perform the method according to the embodiments of the present invention.
In an exemplary embodiment of the present invention, a computer-readable storage medium having stored thereon a program product capable of implementing the method described above in the present specification is also provided. In some possible embodiments, the various aspects of the invention may also be implemented in the form of a program product comprising program code for causing a terminal device to carry out the steps according to the various exemplary embodiments of the invention as described in the "exemplary methods" section of this specification, when said program product is run on the terminal device.
A program product for implementing the above-described method according to an embodiment of the present invention may employ a portable compact disc read-only memory (CD-ROM) and include program code, and may be run on a terminal device, such as a personal computer. However, the program product of the present invention is not limited thereto, and in this document, a readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The program product may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium can be, for example, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium would include the following: an electrical connection having one or more wires, a portable disk, a hard disk, random Access Memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
The computer readable signal medium may include a data signal propagated in baseband or as part of a carrier wave with readable program code embodied therein. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A readable signal medium may also be any readable medium that is not a readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.
Program code embodied on a readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.
Program code for carrying out operations of the present invention may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C++ or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device, partly on a remote computing device, or entirely on the remote computing device or server. In the case of remote computing devices, the remote computing device may be connected to the user computing device through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computing device (e.g., connected via the Internet using an Internet service provider).
Furthermore, the above-described drawings are only schematic illustrations of processes included in the method according to the exemplary embodiment of the present invention, and are not intended to be limiting. It will be readily appreciated that the processes shown in the above figures do not indicate or limit the temporal order of these processes. In addition, it is also readily understood that these processes may be performed synchronously or asynchronously, for example, among a plurality of modules.
Other embodiments of the application will be apparent to those skilled in the art from consideration of the specification and practice of the application disclosed herein. This application is intended to cover any variations, uses, or adaptations of the application following, in general, the principles of the application and including such departures from the present disclosure as come within known or customary practice within the art to which the application pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the application being indicated by the following claims.
Claims (9)
1. A robot trajectory planning method, comprising:
And a child node expansion step: the method comprises the steps that a starting node is used as a first father node to be placed in an open list formed by a plurality of three-dimensional grids in a three-dimensional grid graph, and a plurality of child nodes corresponding to the first father node are expanded in the open list; one three-dimensional grid corresponds to one node, the three-dimensional grid is obtained by dividing a section to be searched based on a three-dimensional space, and the three-dimensional space is constructed based on the length, width and height of an obstacle in the section to be searched;
The current child node determining step: the first father node is put into a closed list, and a first current child node corresponding to the first father node is determined in each child node by taking the minimum total cost value as a principle; wherein the total cost value includes an accumulated cost value and an estimated cost value;
the current child node storing step: when the accumulated cost value of the first current sub-node is determined to be smaller than a preset cost value, the first current sub-node is stored in a target sub-node list, and the preset cost value is replaced by the accumulated cost value of the first current sub-node;
The circulation steps are as follows: sequentially cycling the child node expanding step, the current child node determining step and the current child node storing step, and judging whether the cycle number is larger than the preset number;
Track planning step: when the cycle times are not less than the preset times and the last first current sub-node is not the termination node, planning the track of the robot according to each first current sub-node in the target sub-node list; and when the cycle times are not less than the preset times and the last first current child node is a termination node, planning the track of the robot according to each father node in the closed list.
2. The robot trajectory planning method of claim 1, further comprising:
Three-dimensional space construction: taking the length and the width of the obstacle included in the interval to be searched as a two-dimensional plane X axis and a Y axis, taking the height of the obstacle as a time T axis, and constructing the three-dimensional space according to the X axis, the Y axis and the T axis;
The three-dimensional grid diagram construction step: dividing the interval to be searched into a plurality of three-dimensional grids with the same volume based on the three-dimensional space, and obtaining the three-dimensional grid map according to each three-dimensional grid;
a starting node and a terminating node determining step: and determining a starting node and a terminating node according to the current position of the robot in the interval to be searched and the target position of the target object in the interval to be searched.
3. The robot trajectory planning method of claim 1, wherein expanding a plurality of child nodes corresponding to the first parent node in the open list comprises:
Expanding child nodes corresponding to the first father node according to a preset expansion rule according to the position of the first father node in the open list; wherein the number of the child nodes corresponding to the first parent node is not more than nine.
4. The robot trajectory planning method of claim 1, wherein determining a first current child node corresponding to the first parent node on a basis of a minimum total cost value among the child nodes includes:
Calculating an accumulated cost value from the first parent node to each of the child nodes, and calculating an estimated cost value from each of the child nodes to the termination node;
and obtaining the total cost value among all the child nodes according to all the accumulated cost values and all the estimated cost values, and taking the child node with the minimum total cost value as the first current child node.
5. The method of claim 4, wherein calculating an estimated cost value from each of the child nodes to the termination node comprises:
configuring weight parameters of each child node and each termination node on a time T axis;
and obtaining the estimated cost value according to the weight parameters, the child nodes and the Manhattan distance between the termination nodes.
6. The robot trajectory planning method of claim 1, further comprising, after the trajectory planning step:
And (3) adaptability evaluation: and carrying out adaptability evaluation on the track according to the length of the track and the distance between the tail end position of the track and the termination node.
7. A robot trajectory planning device, comprising:
The child node expansion module is used for placing the initial node serving as a first father node into an open list formed by a plurality of three-dimensional grids in the three-dimensional grid graph, and expanding a plurality of child nodes corresponding to the first father node in the open list; one three-dimensional grid corresponds to one node, the three-dimensional grid is obtained by dividing a section to be searched based on a three-dimensional space, and the three-dimensional space is constructed based on the length, width and height of an obstacle in the section to be searched;
The current child node determining module is used for placing the first father node into a closed list, and determining a first current child node corresponding to the first father node in each child node by taking the total cost value as the minimum principle; wherein the total cost value includes an accumulated cost value and an estimated cost value;
The current sub-node storage module is used for storing the first current sub-node into a target sub-node list when the accumulated cost of the first current sub-node is smaller than a preset cost, and replacing the preset cost by utilizing the accumulated cost of the first current sub-node;
the circulation module is used for sequentially circulating the operations executed by the child node expansion module, the current child node determination module and the current child node storage module and judging whether the circulation times are larger than preset times or not;
The track planning module is used for planning the track of the robot according to each first current sub-node in the target sub-node list when the cycle times are not less than the preset times and the last first current sub-node is not a termination node; and when the cycle times are not less than the preset times and the last first current child node is a termination node, planning the track of the robot according to each father node in the closed list.
8. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the robot trajectory planning method of any one of claims 1-6.
9. An electronic device, comprising:
A processor; and
A memory for storing executable instructions of the processor;
wherein the processor is configured to perform the robot trajectory planning method of any one of claims 1-6 via execution of the executable instructions.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911285207.7A CN112985397B (en) | 2019-12-13 | 2019-12-13 | Robot track planning method and device, storage medium and electronic equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911285207.7A CN112985397B (en) | 2019-12-13 | 2019-12-13 | Robot track planning method and device, storage medium and electronic equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112985397A CN112985397A (en) | 2021-06-18 |
| CN112985397B true CN112985397B (en) | 2024-04-19 |
Family
ID=76342152
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911285207.7A Active CN112985397B (en) | 2019-12-13 | 2019-12-13 | Robot track planning method and device, storage medium and electronic equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112985397B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114589697B (en) * | 2022-03-22 | 2024-05-28 | 海风智能科技(浙江)有限公司 | Intelligent disinfection inspection environment adjusting robot and control method |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20080093580A (en) * | 2007-04-17 | 2008-10-22 | 한국전자통신연구원 | Path finding system and method |
| JP2009288118A (en) * | 2008-05-30 | 2009-12-10 | Univ Waseda | Optimum route searching device, optimum route searching method and program |
| CN105737819A (en) * | 2016-02-25 | 2016-07-06 | 西北工业大学 | Unmanned aerial vehicle three-dimensional airway planning method based on space compression and table-lookup calculation |
| CN107345815A (en) * | 2017-07-24 | 2017-11-14 | 东北大学 | A kind of home-services robot paths planning method based on improvement A* algorithms |
| CN107631734A (en) * | 2017-07-21 | 2018-01-26 | 南京邮电大学 | A kind of dynamic smoothing paths planning method based on D*_lite algorithms |
| CN107806877A (en) * | 2017-10-11 | 2018-03-16 | 湖北工业大学 | A kind of trajectory planning optimization method of four rotor wing unmanned aerial vehicles based on ant group algorithm |
| CN108469827A (en) * | 2018-05-16 | 2018-08-31 | 江苏华章物流科技股份有限公司 | A kind of automatic guided vehicle global path planning method suitable for logistic storage system |
| CN109344208A (en) * | 2018-08-14 | 2019-02-15 | 北京奇虎科技有限公司 | Path query method, apparatus and electronic equipment |
| CN109540146A (en) * | 2018-11-29 | 2019-03-29 | 珠海格力智能装备有限公司 | Path planning method and device |
| CN109947120A (en) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | Paths planning method in warehousing system |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108228676B (en) * | 2016-12-22 | 2021-08-13 | 腾讯科技（深圳）有限公司 | Information extraction method and system |
| US11530921B2 (en) * | 2018-09-28 | 2022-12-20 | Intel Corporation | Method of generating a collision free path of travel and computing system |
-
2019
- 2019-12-13 CN CN201911285207.7A patent/CN112985397B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20080093580A (en) * | 2007-04-17 | 2008-10-22 | 한국전자통신연구원 | Path finding system and method |
| JP2009288118A (en) * | 2008-05-30 | 2009-12-10 | Univ Waseda | Optimum route searching device, optimum route searching method and program |
| CN105737819A (en) * | 2016-02-25 | 2016-07-06 | 西北工业大学 | Unmanned aerial vehicle three-dimensional airway planning method based on space compression and table-lookup calculation |
| CN107631734A (en) * | 2017-07-21 | 2018-01-26 | 南京邮电大学 | A kind of dynamic smoothing paths planning method based on D*_lite algorithms |
| CN107345815A (en) * | 2017-07-24 | 2017-11-14 | 东北大学 | A kind of home-services robot paths planning method based on improvement A* algorithms |
| CN107806877A (en) * | 2017-10-11 | 2018-03-16 | 湖北工业大学 | A kind of trajectory planning optimization method of four rotor wing unmanned aerial vehicles based on ant group algorithm |
| CN108469827A (en) * | 2018-05-16 | 2018-08-31 | 江苏华章物流科技股份有限公司 | A kind of automatic guided vehicle global path planning method suitable for logistic storage system |
| CN109344208A (en) * | 2018-08-14 | 2019-02-15 | 北京奇虎科技有限公司 | Path query method, apparatus and electronic equipment |
| CN109540146A (en) * | 2018-11-29 | 2019-03-29 | 珠海格力智能装备有限公司 | Path planning method and device |
| CN109947120A (en) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | Paths planning method in warehousing system |
Non-Patent Citations (3)
| Title |
|---|
| Pavel Vajsar ; Jiri Hosek ; Karol Molnar ; Milan Bartl.Advanced trajectory management techniques for mobile nodes in OPNET Modeler environment.2012 35th International Conference on Telecommunications and Signal Processing.2012,全文. * |
| 基于蚁群算法的四旋翼航迹规划;莫宏伟;马靖雯;;智能***学报;20160430;11(02);全文 * |
| 基于踪片Tracklet关联的视觉目标跟踪:现状与展望;刘雅婷，王坤峰，王飞跃;自动化学报;20171130;第43卷(第11期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112985397A (en) | 2021-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110806218B (en) | Parking path planning method, device and system | |
| US11300964B2 (en) | Method and system for updating occupancy map for a robotic system | |
| Kallmann | Dynamic and robust local clearance triangulations | |
| US20200097008A1 (en) | Action Planning System and Method for Autonomous Vehicles | |
| US8935096B2 (en) | Apparatus for fast path search by learning heuristic function and method thereof | |
| CN112529254B (en) | Path planning method and device and electronic equipment | |
| Kallmann et al. | Navigation meshes and real-time dynamic planning for virtual worlds | |
| Liu et al. | PSO-based power-driven X-routing algorithm in semiconductor design for predictive intelligence of IoT applications | |
| CN111680747B (en) | Method and apparatus for closed loop detection of occupancy grid subgraphs | |
| CN109341698B (en) | Path selection method and device for mobile robot | |
| CN112985397B (en) | Robot track planning method and device, storage medium and electronic equipment | |
| CN114261400A (en) | Automatic driving decision-making method, device, equipment and storage medium | |
| CN112084853A (en) | Footprint prediction method, footprint prediction device and humanoid robot | |
| Minakova et al. | Scenario based run-time switching for adaptive CNN-based applications at the edge | |
| Chand et al. | A two-tiered global path planning strategy for limited memory mobile robots | |
| CN112182819A (en) | Structure topology optimization method and system based on weighted graph and readable storage medium | |
| CN113885531B (en) | Method for mobile robot, circuit, medium, and program | |
| González | Fast Marching Methods in path and motion planning: improvements and high-level applications | |
| Opoku et al. | The Ar-Star (Ar) Pathfinder | |
| CN113849498B (en) | Index construction and query method | |
| CN114721388A (en) | Cleaning path planning method for self-moving robot | |
| CN113790729A (en) | Unmanned overhead traveling crane path planning method and device based on reinforcement learning algorithm | |
| CN112595333A (en) | Road navigation data processing method and device, electronic equipment and storage medium | |
| WO2020250101A1 (en) | Methods and systems for path planning in a known environment | |
| Neuman et al. | Anytime policy planning in large dynamic environments with interactive uncertainty |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |