WO2026010520A1 - Method and system for collision avoidance for agricultural machines - Google Patents

Abstract

A method for path planning for a plurality of machines includes determining a path of one of a plurality of machines from a start position to an end position and modifying the path based on obstacles. Movement of the others of the plurality of machines is prevented based on the plurality of machines interfering with movement of the one of the plurality of machines. Movement can be prevented based on a priority of each of the plurality of machines. The path can be determined based on kinematic constraints of the one of the plurality of machines. The path can also be determined based on one or more of the cost of the path compared to costs of other paths and straight and curved segments based on kinematic constraints.

Description

METHOD AND SYSTEM FOR COLLISION AVOIDANCE FOR AGRICULTURAL MACHINES

BACKGROUND

[0001] Path planning for a plurality of machines (e.g., agricultural, construction, robots) requires consideration of various factors in order to facilitate movement of each of the machines from its start position to its end position along its path. Although there are numerous algorithms for path planning, what is needed is a path planning algorithm that takes into account movement of a plurality of machines in the same area, obstacle avoidance, and a method for reducing the computing power required for performing the algorithm.

FIELD OF THE INVENTION

[0002] The present disclosure relates generally to machine control, and more particularly to decentralized collision avoidance for a swarm of wheeled agricultural machines.

SUMMARY

[0003] A method determining a physical path represented by a plurality of vertices for one of a plurality of machines includes the steps of selecting a first vertex representing a first location of the one of the plurality of machines and identifying a plurality of next vertices, each of the plurality of next vertices representing a possible second location of the one of the plurality of machines, that the one of the plurality of machines can travel to based on the first vertex and discrete motion patterns of the machine. The method also includes the steps of selecting a second vertex from the plurality of next vertices based on a priority of the second vertex and controlling the one of the plurality of machines in accordance with a determined path comprising the first vertex and the second vertex. The first vertex can represent a starting position of the one of the plurality of machines and the plurality of machines can operate in an allowed area. A priority of the second vertex can be identified in an OpenList that stores a list of vertices that have not been analyzed. The plurality of next vertices can be stored in a priority queue. The plurality of next vertices stored in the priority queue can be selected from a plurality of vertices stored in an OpenList that stores a list of vertices that have not been analyzed. The respective priority of each of the plurality of next vertices can be based on a distance from a starting position of a respective one of the plurality of machines to the respective one of the plurality of next vertices. The respective priority of each of the plurality of next vertices can be further based on an estimated distance from the respective one of the plurality of next vertices to an end position of the respective one of the plurality of machines. The determined path is complete when the second vertex is an end position of a respective one of the plurality of machines. The completed path can be compared to other paths for the respective one of the plurality of machines to determine a shortest path from a starting position to an end position of the respective one of the plurality of machines. The second vertex can be selected based whether there is an obstacle blocking a path for the one of the plurality of machines to the second vertex.

[0004] An apparatus having memory storing computer program instructions and a computer readable medium storing instructions for determining a physical path represented by a plurality of vertices for one of a plurality of machines are also described herein.

BRIEF DESCRIPTION OF THE DRAWINGS

[0005] FIG. 1 shows a system for planning paths for each of a plurality of machines;

[0006] FIG. 2 shows a structural diagram of the decentralized planner unit according to one embodiment;

[0007] FIG. 3 shows a high-level schematic of a computer for implementing method and systems described herein;

[0008] FIG. 4 illustrates a search algorithm illustrating the progression of selection of nodes; [0009] FIG. 5 A shows a visualization of a heuristic map according to one embodiment;

[0010] FIG. 5B shows an example of simple motions in two-dimensional space according to one embodiment;

[0011] FIG. 6A shows an example of applying a Reeds-Shepp heuristic to path planning;

[0012] FIG. 6B shows an example of applying a 2-dimensional map heuristic to path planning;

[0013] FIG. 7 illustrates a thinning procedure according to one embodiment;

[0014] FIGS. 8A-8D show the thinning process over several iterations;

[0015] FIGS. 9A and 9B show a comparison of path planning algorithms with and without thinning;

[0016] FIGS. 10A and 10B show examples of obstacle collision checking;

[0017] FIGS. 11 A and 1 IB show examples of the process for verification of geometric constraints; [0018] FIGS. 12A-12C show examples of how the search algorithms are used for path planning; [0019] FIG. 13 shows a plurality of machines with each machine having a priority; and [0020] Fig. 14 shows a flow chart of a method according to one embodiment.

DETAILED DESCRIPTION

[0021] A method and system for decentralized collision avoidance for a plurality (e.g. a swarm) of wheeled machines (e.g., agricultural, construction, robots) includes planning paths for each of the plurality of machines from a start position to an end position. In one embodiment, each of a plurality of machines has a control module configured to control its operation and communicate with other devices to facilitate path planning. Autonomous machines require information to operate safely and efficiently. Such information includes information about each of a plurality of vehicles’ operation and information about the area in which the vehicles operate. The information about each of the plurality of vehicles’ operation can include location, steering radius, speed, dimensions (e.g., size and shape), capabilities, etc. The information about the area includes coordinates defining the area, features in the area, fixed obstacles in the area, etc. The information about each of the plurality of vehicles’ operation can also include a respective path for each of the plurality of vehicles. The information about the vehicle’s operation and the information about the area can be used by a path planning system and method to plan paths for each of the plurality of vehicles.

[0022] Path planning for autonomous machines is well-known but the associated algorithms used for path planning are not efficient for certain applications and, as such, are not suitable for all applications. The method and system described herein includes dynamic planning of maneuvers for each of the plurality of machines based on the available area for operations and the movement constraints of each of the plurality of machines. The method and system further includes autonomous maneuvering of machines having a specific type of steering, such as Ackerman steering, and avoiding collisions with other machines and obstacles.

[0023] In one embodiment, path planning uses a two-dimensional grid comprising a plurality of cells. The grid is used to represent an area in which a plurality of machines operate. The two perpendicular axes of the grid are referred to as the X axis and Y axis. Using this grid allows cells to be identified based on their position along the X axis and Y axis. The current and possible future positions of a machine on the grid are referred to as nodes or vertices. A machine located at a particular vertex can travel to other adjacent vertices based on kinematic constraints of the machine. For example, a machine can travel straight, curve left, or curve right. Thus, a machine traveling at a particular speed over a period of time will traverse a certain distance in a direction based on whether the machine is traveling straight, turning left, or turning right. The path segment traveled from one vertex to the next vertex is referred to as a discrete planning pattern or motion primitive. The discrete planning pattern or motion primitive are segments of circles and straight lines where the segments of circles are based on the turning radius of the machine whose path is being planned.

[0024] FIG. 1 shows system 100 for planning paths for each of a plurality of machines 101, 103, 105 from a start position of a path to an end position of the path. Although three machines are shown, the number of machines can be more or less than three. The components of each machine 101, 103, and 105 are identical but have been omitted from being shown in machines 103 and 105 for clarity. Each of the plurality of machines comprises a control module 102 having several components. Control module 102 comprises path planner unit 106, logic of machine state unit 108, safety unit 110 and control unit 112. Path planner unit 106 and safety unit 110 are in communication with logic of machine state unit 108. Path planner unit 106 and safety unit 110 of control module 102 are also in communication with occupancy grid unit 114 and localization unit 116. Localization unit 116 is in communication with decentralized planner unit 118 which is also in communication with logic of machine state unit 108 of control module 102. Decentralized planner unit 118 is also in communication with Message Queuing Telemetry Transport (MQTT) bridge 120 which is in communication with MQTT broker 122. Control unit 112 of control module 102 is in communication with actuators 104 which are located on a machine and are used to operate the machine in a desired manner. For example, actuators 104, in one embodiment, comprise actuators to propel and steer the machine. In one embodiment, each machine makes all of its own decisions based on data from its sensors and the data received from other machines. In one embodiment, the components shown in FIG. 1 utilize a Robot Operating System (ROS) framework which works as a meta-operating system based on Linux OS. The ROS includes a set of tools, libraries, and drivers that allow a user to build complex software for machines (e.g., agricultural, construction, robots) that run on a variety of platforms. One aspect of ROS is the modularity of the software which is divided into various modules, between which communication is carried out. [0025] In one embodiment, control module 102 is configured to create a control action that is output to actuators 104. Control module 102 receives data about a position, orientation, and speed of its respective machine from localization unit 116 and also information pertaining to obstacles located in the area in which the machine is operating from the occupancy grid unit 114.

[0026] Each machine is also in communication with decentralized planner unit 118 which provides communication among a plurality of machines and is configured to ensure that collective behavior of the machines occurs as desired.

[0027] FIG. 2 shows a structural diagram of decentralized planner unit 118. In one embodiment, decentralized planner unit 118 is the unit that processes information from all the machines involved in the work. Its main task is to allow several machines to work simultaneously and resolve collisions. In one embodiment, decentralized planner unit 118 collects data from localization unit 116, control module and other machines and based on this information it is possible to categorize machines as those who will stay in On Way and those who will stay in Wait mode based on its priority. If a machine has conflict with a higher priority machine it stands (Wait mode) while the other machines continue to move to the target position (OnWay mode). In one embodiment, a second function of a decentralized planner unit is to provide additional information to path planning algorithms about obstacles clouds which are formed from other machines. The information that is transmitted over MQTT bridge 120 and MQTT broker 122 contains data about the position, orientation, and velocity of each machine as well as its unique identifier and the internal state of decentralized planner module 118. Each machine stores geometric parameters corresponding to the received machine identifier in a table. These geometric parameters are used to identify potential collisions between different machines. FIG. 13 shows an example of a collision checking procedure and identifying machine mode. A machine’s number in FIG. 13 correspond to machine priority. A directed graph between collision machines is built by drawing an edge from a higher-priority machine to a lower-priority machine. Based on this information, the mode of each machine can be explicitly determined. Incoming data flow from the localization module, control module, and mqtt bridge the decentralized planner state can take one of the following values {ToRoute, OnStartWait, OnRoute, Wait, Stop}. ToRoute 1302 and OnRoute 1304 statuses mean that the movement of the machine is in a normal mode before and after entering the main route. Wait 1306 state means the presence of higher priority machines interfering with movement. When detecting this status, the control module is set to a Stop mode. OnStartWait 1310 status exists for waiting while other machines will reach the start route position to simultaneously start operation. The Stop 1308 state indicates that the machine is in forced stop for some reason. In the case of the Stop 1308 state, the priority of the machine is considered to be zero. Decentralized planner unit’s input contains information about current control state and control output from control unit, position and orientation of the machine from localization module, and the same information about other machines from MQTT (Message Queuing Telemetry Transport) bridge. Decentralized planner unit’s output contain state of unit and obstacles cloud which are formed by other machines. This information is used in the control module.

[0028] Communication among the machines is performed using the MQTT protocol. This protocol facilitates streaming data between devices with limited CPU power and/or battery life as well as networks with expensive or low bandwidth, unpredictable stability, or high latency. The protocol is ideal for distributed one-to-many communications and disconnected applications. There are various open-source message brokers in different programming languages that implement MQTT protocols. For example, Eclipse Mosquitto is used in one embodiment. A communication system built on MQTT consists of an issuing server, a broker server and one or more clients, which in our case are machines. The clients can be both subscribers and publishers of messages.

[0029] Each of the components of system 100 shown in FIG. 1, as well as other devices described herein, can be implemented using a computer. A high-level block diagram of such a computer is illustrated in FIG. 3. Computer 202 contains a processor 204 which controls the overall operation of the computer 202 by executing computer program instructions which define such operation. The computer program instructions may be stored in a storage device 212, or other computer readable medium (e.g., magnetic disk, CD ROM, etc.), and loaded into memory 210 when execution of the computer program instructions is desired. Thus, the method steps of FIG. 14, as well as other methods and algorithms described herein, can be defined by the computer program instructions stored in the memory 210 and/or storage 212 and controlled by the processor 204 executing the computer program instructions. For example, the computer program instructions can be implemented as computer executable code programmed by one skilled in the art to perform an algorithm defined by the method steps of FIG. 14, as well as other methods and algorithms described herein. Accordingly, by executing the computer program instructions, the processor 204 executes an algorithm defined by the method steps of FIG. 14 or other methods and algorithms described herein. The computer 202 also includes one or more network interfaces 206 for communicating with other devices via a network. The computer 202 also includes input/output devices 408 that enable user interaction with the computer 202 (e.g., display, keyboard, mouse, speakers, buttons, etc.) One skilled in the art will recognize that an implementation of an actual computer could contain other components as well, and that FIG. 3 is a high level representation of some of the components of such a computer for illustrative purposes.

[0030] In one embodiment, an algorithm for path planning is used to plan a path a machine will traverse from an initial state (e.g., initial position) {X_start, Y_start, 0_start} ^{to a} ^^ina' ^state (^e-g-> ^^ina' position or end position) (X_end, Y_end, 0_end}- The algorithm ensures the avoidance of obstacles, is based on kinematic constraints of the machine, and maintains the machine’s location inside the allowed area of operation. {X_start, Y_start] is the initial position of the rear axle center of the wheeled machine in coordinates and 0_start is the orientation angle of the machine.

[0031] The path planning algorithm described herein belongs to a class of discrete path planning algorithms and is similar to the well-known A* algorithm. A key feature of the path planning algorithm is that the search is not based on a predefined graph but is instead based on accumulated information about the vertices visited (also referred to as vertices that have been analyzed) and the vertices to be considered to be visited (also referred to vertices that are to be analyzed). In one embodiment, this information is stored in a dynamic memory of control unit 106. An OpenList structure stores the vertices (also referred to as nodes) to be considered to be visited in the future, and a ClosedList structure stores the already visited vertices.

[0032] In one embodiment, the path planning process is carried out using discrete planning patterns, which are realizable motions for the machine, from which the path is then formed. In the path planning algorithm, these patterns are segments of circles and straight lines.

[0033] Vertices that are retrieved from the OpenList structure according to a certain priority, which will be described below, can identify adjacent vertices that are placed back into the OpenList based on motion primitives. The condition for considering a vertex is its absence in the ClosedList or the existence of a better path leading to this vertex.

[0034] In one embodiment, a data structure storing node information shown in the upper left of Fig 4 (e.g., vertex information) includes the following data: 1) a location within a cell; 2) a distance traveled from a starting search position to a current cell; 3) a cost which was spent to reached a current position (this cost will be optimized); 4) a type of movement (e.g., motion primitive) that was used to move to that cell from a previous cell; and 5) an identifier of the previous cell from which the movement was made. Thus, when the target is reached, the complete path can be reconstructed using the search history inside ClosedList.

[0035] FIG. 4 illustrates a search algorithm illustrating the progression of selection of nodes (i.e., vertices). Grid 300 comprises a plurality of cells with the vertical axis designated “X” and the horizontal axis designated “Y”. At step 1 , shown in the upper left of the grid, node 302 is identified as a node that is on the ClosedList of a NodeList meaning that node 302 has been examined. Nodes 304A-304F are identified as nodes that are on the OpenList of the NodeList meaning that those nodes have not been examined. The next node to be examined can be any one of nodes 304A- 304F. At step 2, node 304C has been chosen as the next node to be examined which results in the next set of nodes that can be chosen as nodes 306A-306D. At step 3, node 306B has been chosen as the next node to be examined which results in the next set of nodes that can be chosen as nodes 308A-308E. Step 4 located in the lower right of grid 300 shows that if node 308A is chosen, then motion of an associated machine can continue in the direction of arrow 312. Step 4 also shows that if node 304B were chosen previously, it would result in nodes 310A-310E being available for selection and analysis. Grid 300 is stored in hash table structure which are shown in FIG. 4 as NodeList. The key of the hash table is node identifier and the value which corresponds to this key is vertex Node. This identifier N_id for vertex with position [x, y, 0} is calculated by this formula dimensions, y_min, y_maxi y_mini y_max bounds values of planning allowed space.

[0036] In one embodiment, a priority queue Q is used to implement the node selection scheme described above where identifiers of OpenList vertices with their priority X_end) are stored. Identifiers of vertices retrieved from the OpenList correspond to the highest priority values from the priority queue Q. Choosing an appropriate heuristic to set node priorities can produce a search directed to the target, which significantly speeds up the algorithm's running time.

[0037] The described algorithm allows for building a path composed of discrete motion patterns including line segments, and circles. In one embodiment, in order to have a smooth connection between this path and final state X_end, analytical connection by the well-known Dubins or Reeds- Shepp paths is used. [0038] In one embodiment, when avoiding obstacles, position X_end is selected on the path behind an obstacle based on safety requirements and optimization of an untreated section of route.

[0039] Additions and improvements to the algorithm described herein are used to solve the following problems: 1) calculation of the h(X,X_end)', 2) increased memory consumption and search time with an increasing number of iterations in complex constraint configurations; 3) choosing an efficient way of checking for intersections with obstacles; 4) efficiently taking into account geometric constraints for planning a path.

[0040] The solution to the first problem of calculation of the h(X, X_end) determines the direction for the search. In one embodiment, the search has inverse start and end/goal points (i.e., the start of the search is located at the end/goal point and vice versa). For a simple environment, the Reeds- Shepp path length between X and X_end can be used for h(X, X_end) ■ However, it is not suitable for complex boundaries and restrictions. Instead, heuristic mapping is used for complex boundaries and restrictions.

[0041] Heuristic mapping is used to cover the space in a way to know the distance from each cell to the target. This distance does not take into account kinematics of the machine, but it gives an estimated direction which significantly increases the speed and efficiency of the algorithm. FIG. 5A shows visualization 400 of a heuristic map. To build such a heuristic map, a Dijkstra or A* algorithm in two-dimensional space is used based on simple motion patterns 402 as shown in FIG. 5B. Boundaries 404, 406 show the boundary of an area of interest. Depth of shading corresponds to values of heuristic map. Light shading corresponds to higher distance to goal while dark shading corresponds to lower distance. The path planning algorithm has the same working principle as a hybrid A* algorithm and the coordinate system for calculating the index of the cell is the same, however it is used in two-dimensional space and to identify cell [x, y] -> N_id ^2d the following formula is used.

[0043] FIG. 6 A shows an example of applying a Reeds-Shepp heuristic so machine 502 remains within boundaries 504. FIG. 6B shows an example of applying a 2-dimensional map heuristic so machine 506 remains within boundaries 508. Area 510 shown in FIGS. 6A and 6B represent the areas that the algorithm has considered to find a path to the goal point 512. [0044] In one embodiment, after search completion, a ClosedList of a two-dimensional search algorithm is used as a heuristic map because it contains the list of visited nodes, and node cost corresponds to the distance to the goal.

[0045] Note that FIG. 5A shows that the strategy of using Reeds-Shepp path length as h(X,X^) does not work in maze scenarios but the path planning algorithm described herein produces usable results.

[0046] To solve the second problem of increased memory consumption and search time with an increasing number of iterations in complex constraint configurations, a scheme of thinning the accumulated information is used. In one embodiment, after a certain number of iterations, some of the vertices which have smaller distances to the start point are removed from Q structures of the priority queue Q. Corresponding nodes are also removed from the OpenList of the NodeList. Suitable nodes for continuing search are placed in additional priority queue W. During the search, W is filled by the same nodes as Q but the priority of inserted elements is dependent on the vertex distance to the start position. FIG. 7 illustrates the principle of the thinning procedure. Q priority queue 602 is shown having particular contents before W priority queue 604 is used to produce Q after thinning 606. Similarly, OpenList hash table 608 shown having particular contents before W priority queue 604 is used to produce OpenList after thinning 610. In FIG. 7, k is the thinning size. In one embodiment, if for a given number of iterations it is not possible to reach the final goal, the search starts over, but with the already existing knowledge about the environment. The most relevant information is used to continue the search, the rest is forgotten (i.e., removed from queues as necessary. In scenarios with complex constraints, this speeds up the search process, but it can also affect optimality of the path. A compromise between the two effects is achieved by fitting the coefficients so that the results of the algorithm satisfy the terms of the specification parameters. In one embodiment, these parameters are the number of iterations when the procedure is repeated and the thinning number (i.e., the OpenList and Q size after the thinning procedure). FIG. 6 shows the visited nodes after each thinning procedure. Comparison of search procedure with and without thinning is also shown in FIG. 6. In one embodiment, the algorithm with thinning took 3100 iterations and used several times less time and memory than the algorithm without thinning which takes 8500 iterations. FIG. 7 schematically illustrates the process of selecting vertices to continue the search. First k identifiers from priority queue W are used to form new OpenList and priority queue Q. [0047] FIGS. 8A-8D show the thinning process for each 1000 iterations with a thinning size of 100. Line 802 and line 804 correspond to the borders which form geometrical constraints. Line 806 is machine route. Shading 808 corresponds to occupied space (i.e., visited nodes from OpenList during the search procedure). Shading 810 corresponds to OpenList nodes which are used after thinning procedure. FIG. 8 A shows 0-1000 iterations, FIG.8B shows 1000-2000 iterations, FIG. 8C shows 2000-3000 iterations, and FIG. 8D shows the result after 3200 iterations. FIGS. 9 A and 9B show a comparison of algorithms with and without thinning. FIG. 9 A shows the result of the algorithm with thinning after 3080 iterations and 7 milliseconds. FIG. 9B shows the result of the algorithm after 8230 iterations and 20 milliseconds. After applying the thinning procedure occupied space and computational time is several times less than without it. The third problem of choosing an efficient way for checking for intersections with obstacles is solved, in one embodiment, as follows. Obstacles are defined as a point cloud. To avoid collisions, the planned path must be located at the required distance from the obstacles, which is controlled by an obstacle radius parameter. For each iteration of the planning algorithm, new positions of the machine are checked for intersection with an obstacle point cloud. The OpenList is populated with nodes whose positions meet the safety criterion which means that a machine frame which corresponds to some node position {x, y, 0} must lie beyond a given distance from an obstacle cloud. The point cloud is stored in the well-known KDTree data structure and the intersection check is performed only with obstacles lying within a given distance to the center of the machine. This procedure has a logarithmic rather than linear complexity on the number of points, which can significantly speed up the search procedure.

[0048] FIGS. 10A and 10B shows an example of obstacles collision checking with machine 900 moving from a first position shown in FIG. 10A to a second position shown in FIG. 10B. First and second position correspond to neighbor vertex in graph. As shown in FIG. 10A, obstacles 902-914 are shown located in an area in which machine 900 is operating with machine 900 shown in a first position in which obstacles 904, 906, 908, and 912 are located within a particular radius of machine 900. Obstacles 904, 906, 908, and 912, in one embodiment, are required to be checked to determine if machine 900 will collide with them based on the proposed movement of machine 900. Choosing the obstacles for analysis takes logarithmic complexity. FIG. 10B shows machine 900 in a second position in which obstacles 902, 904, 906, 908, and 912 are located within a particular radius of machine 900 and are required to be checked to determine if machine 900 will collide with them. [0049] The fourth problem of efficiently taking into account geometric constraints for planning a path is solved, in one embodiment, as follows. Geometric constraints are set by a system of polygons. Among them there is a region fi_R , defining the outer contour of the allowed region, as well as internal obstacles fl_0N , all points of the resulting path must belong to a region of space fl = fj_R\ (f]₀₁ U fl₀₂ , . . Uft_Ov) which means that each point should be inside fi_R and outside each obstacle polygon v{x, y) G S, {x, y} G Q_R, {x, y} G fl₀₁ U fl₀₂ , . . \JQ_0N ■ Checking procedure is conducted for all vertex has linear complexity O(n) where n is polygon size. One possible method to determine if a given point lie inside a polygon is to find the nearest projection on a polygon from a given point. A vector to the nearest projection _m is defined as well as a unit normal N at this point directed inward of the polygon. The value of the scalar product (m, N_) determines the distance to the boundary and on which side of it the point in question is located. Visualization of the described procedure is shown in FIGS. 11 A.

[0050] In one embodiment, an algorithm is used to speed up verification of geometric constraints. During an initialization process, a special hash table is created in memory. The key in this table is the cell index N_id ^2d , and the values are the indices of the nearest edge number of the polygon defining the allowed area. To save memory, only the numbers of the cells lying in the immediate vicinity of the boundary are stored. The same principle is applied in case of obstacles, given by a polygon. The length of the elementary step in the search process must be less than the width of the zone that is formed by the "painted" cells, so if the start and end positions of the machine lie within the allowed zone, then all intermediate positions that lie in the "unpainted zone" can be considered to lie within the allowed zone P. Otherwise, it is necessary to make a check only for a given edge of the polygon. Thus checking [_x, y] G ft takes a constant time, which can significantly reduce search time.

FIGS. 11A and 1 IB show examples of the process of building a described hash table for verification of geometric constraints. Rectangles correspond to the painted zone around the contour of the allowed region.

[0051] The resulting path can lie extremely close to the boundary of the allowed area, but due to inaccurate path tracking, the machine may leave the allowed boundary of the planning area by some distance. Because of this, when rescheduling a path, it is possible that (X_start, Y_start] G ft , which prevents the algorithm from terminating correctly. To solve this problem, in one embodiment, the following method is used. An extended area is defined as c fl . This area {x,y} c jQ* \/J is the difference of these sets. During a search, new nodes whose position belongs to this set can be added to the OpenList if the previous node position also belongs to it. That is, these transitions are possible fl* => fl and jQ* =* £i* while fl => fl* is not.

[0052] A cell may be visited several times during a search. Storing and retrieving the information about whether the position corresponding to a given cell is safe, as well as storing and retrieving the heuristic estimation for a curtain cell can avoid re-computation and can improve time performance of the algorithm.

[0053] FIGS. 12A-12C show examples of how the search algorithms are used. Typical application scenarios for machines tracking a preplanned path including finding a path to get to the route start position or end position, and avoiding obstacles that impede movement with returning to the route. Lines 1210 correspond to geometrical constraints i.e., obstacles or contour of the allowed region The path planning algorithm can use a prioritized system. The path planning algorithm, in one embodiment, is to evaluate the environment and check for the presence of higher-priority machines which impede movement. If such machines exist, the right of way is given to the higher priority machines which perceive others as static obstacles while less priority machines stand. The described scheme of the path planning algorithm can be represented as an oriented graph, where the vertices are the positions of the machines. Provided a machine collision is possible, the vertices of the graph are connected by edges in the direction from the higher-priority to the lower-priority machine. In case the vertex of the graph corresponding to a certain machine has an incoming edge, then this machine must stop.

[0054] FIG. 13 shows a plurality of machines with each machine having a priority. Machine 1202 has the highest priority (i.e., 9) of the machines shown and therefore is allowed to move. Machine 1204 has the next highest priority (i.e., 7) of the machines shown and its movement does not interfere with Machine 1202 so it is allowed to move. Machine 1206 has the next highest priority (i.e., 5) and then machine 1208 (i.e., 2) and the movement of those machines would interfere with the movement of higher priority machines 1202 and 1204 so machines 1206 and 1208 are prevented from moving until the movement of machines 1202 and 1204 is not inhibited.

[0055] Since each machine has a unique priority, the graph cannot have cycles (i.e., the situation of endless waiting of a group of machines waiting for each other is impossible). [0056] Each machine compares its priority with the priorities of all machines with which a collision is potentially possible. The criterion for a potential machine collision is the intersection of artificial regions that are formed near the machine boundary and depend on the direction, speed of movement and the control action in the form of the trajectory curvature.

[0057] The priority of the k-th machine P^P* depends on its unique identifier id._kid._k, and on the state of the machine received from the control module as shown in the following equation:

[0058] p_k = id_k if state_k = ToRoute or state_k = OnRoute, else P_k = 0

[0059] If a machine cannot continue movement for some reason, its priority becomes 0, and other machines with a lower id will not have a stop reaction near that machine. Priority machines try to continue driving when they encounter less priority machines, bypassing, if necessary, lower- priority machines that are waiting for a safe situation. To provide additional safety when the machines avoid each other, the decentralized planner unit 118 transmits information about closely spaced machines in the form of a point cloud covering the boundary of the machines.

[0060] FIG. 14 shows a flow chart of a method 1400 for determining a physical path represented by a plurality of vertices for one of a plurality of machines according to one embodiment. At step 1402 a first vertex representing a first location of the one of the plurality of machines is selected. At step 1404, a plurality of next vertices that the one of the plurality of machines can travel to based on the first vertex and discrete motion patterns of the machine are identified. Each of the plurality of next vertices represents a possible second location of the one of the plurality of machines. At step 1406, a second vertex is selected from the plurality of next vertices based on a priority of the second vertex. In one embodiment, the first vertex is a starting position of the one of the plurality of machines. At step 1408, the one of the plurality of machines is controlled in accordance with a determined physical path comprising the first vertex and the second vertex. In one embodiment, the priority of the second vertex is identified in an OpenList that contains vertices that have not been analyzed. In one embodiment, the plurality of next vertices are stored in a priority queue. The plurality of next vertices stored in the priority queue can be selected from a plurality of vertices stored in an OpenList that contains vertices that have not been analyzed. In one embodiment, the respective priority of each of the plurality of next vertices is based on a distance from a starting position of a respective one of the plurality of machines to the respective one of the plurality of next vertices. The respective priority of each of the plurality of next vertices can be further based on an estimated distance from the respective one of the plurality of next vertices to an end position of the respective one of the plurality of machines. In one embodiment, the path is complete when the second vertex is an end position of a respective one of the plurality of machines. The completed path can be compared to other paths for the respective one of the plurality of machines to determine a shortest path from a starting position to an end position of the respective one of the plurality of machines. In one embodiment, the second vertex is selected based whether there is an obstacle blocking a path for the one of the plurality of machines to the second vertex.

[0061] The foregoing Detailed Description is to be understood as being in every respect illustrative and exemplary, but not restrictive, and the scope of the inventive concept disclosed herein is not to be determined from the Detailed Description, but rather from the claims as interpreted according to the full breadth permitted by the patent laws. It is to be understood that the embodiments shown and described herein are only illustrative of the principles of the inventive concept and that various modifications may be implemented by those skilled in the art without departing from the scope and spirit of the inventive concept. Those skilled in the art could implement various other feature combinations without departing from the scope and spirit of the inventive concept.

Claims

CLAIMS:

1. A method for determining a physical path represented by a plurality of vertices for one of a plurality of machines, the method comprising: selecting a first vertex representing a first location of the one of the plurality of machines; identifying a plurality of next vertices, each of the plurality of next vertices representing a possible second location of the one of the plurality of machines, that the one of the plurality of machines can travel to based on the first vertex and discrete motion patterns of the machine; selecting a second vertex from the plurality of next vertices based on a priority of the second vertex; and controlling the one of the plurality of machines in accordance with a determined path comprising the first vertex and the second vertex.

2. The method of claim 1, wherein the first vertex represents a starting position of the one of the plurality of machines.

3. The method of claim 1, wherein the plurality of machines operate in an allowed area.

4. The method of claim 1, wherein the priority of the second vertex is identified in an OpenList that stores a list of vertices that have not been analyzed.

5. The method of claim 1 , wherein the plurality of next vertices are stored in a priority queue.

6. The method of claim 5, wherein the plurality of next vertices stored in the priority queue are selected from a plurality of vertices stored in an OpenList that stores a list of vertices that have not been analyzed.

7. The method of claim 1, wherein the respective priority of each of the plurality of next vertices is based on a distance from a starting position of a respective one of the plurality of machines to the respective one of the plurality of next vertices.

8. The method of claim 7, wherein the respective priority of each of the plurality of next vertices is further based on an estimated distance from the respective one of the plurality of next vertices to an end position of the respective one of the plurality of machines.

9. The method of claim 1, wherein the determined path is complete when the second vertex is an end position of a respective one of the plurality of machines.

10. The method of claim 9, wherein the completed path is compared to other paths for the respective one of the plurality of machines to determine a shortest path from a starting position to an end position of the respective one of the plurality of machines.

11. The method of claim 1, wherein the second vertex is selected based whether there is an obstacle blocking a path for the one of the plurality of machines to the second vertex.

12. An apparatus comprising: a processor; and a memory to store computer program instructions, the computer program instructions for determining a physical path represented by a plurality of vertices for one of a plurality of machines, which, when executed on the processor cause the processor to perform operations comprising: selecting a first vertex representing a first location of the one of the plurality of machines; identifying a plurality of next vertices, each of the plurality of next vertices representing a possible second location of the one of the plurality of machines, that the one of the plurality of machines can travel to based on the first vertex and discrete motion patterns of the machine; selecting a second vertex from the plurality of next vertices based on a priority of the second vertex; and controlling the one of the plurality of machines in accordance with a determined path comprising the first vertex and the second vertex.

13. The apparatus of claim 12, wherein the first vertex represents a starting position of the one of the plurality of machines.

14. The apparatus of claim 12, wherein the plurality of machines operate in an allowed area.

15. The apparatus of claim 12, wherein the priority of the second vertex is identified in an OpenList that stores a list of vertices that have not been analyzed.

16. The apparatus of claim 12, wherein the plurality of next vertices are stored in a priority queue.

17. The apparatus of claim 16, wherein the plurality of next vertices stored in the priority queue are selected from a plurality of vertices stored in an OpenList that stores a list of vertices that have not been analyzed.

18. The apparatus of claim 12, wherein the respective priority of each of the plurality of next vertices is based on a distance from a starting position of a respective one of the plurality of machines to the respective one of the plurality of next vertices.

19. The apparatus of claim 18, wherein the respective priority of each of the plurality of next vertices is further based on an estimated distance from the respective one of the plurality of next vertices to an end position of the respective one of the plurality of machines.

20. The apparatus of claim 12, wherein the determined path is complete when the second vertex is an end position of a respective one of the plurality of machines.

21. The apparatus of claim 20, wherein the completed path is compared to other paths for the respective one of the plurality of machines to determine a shortest path from a starting position to an end position of the respective one of the plurality of machines.

22. The apparatus of claim 12, wherein the second vertex is selected based whether there is an obstacle blocking a path for the one of the plurality of machines to the second vertex.

23. A computer readable medium storing computer program instructions for determining a physical path represented by a plurality of vertices for one of a plurality of machines, which, when executed on a processor, cause the processor to perform operations comprising: selecting a first vertex representing a first location of the one of the plurality of machines; identifying a plurality of next vertices, each of the plurality of next vertices representing a possible second location of the one of the plurality of machines, that the one of the plurality of machines can travel to based on the first vertex and discrete motion patterns of the machine; selecting a second vertex from the plurality of next vertices based on a priority of the second vertex; and controlling the one of the plurality of machines in accordance with a determined path comprising the first vertex and the second vertex.

24. The computer readable medium of claim 23, wherein the first vertex represents a starting position of the one of the plurality of machines.

25. The computer readable medium of claim 23, wherein the plurality of machines operate in an allowed area.

26. The computer readable medium of claim 23, wherein the priority of the second vertex is identified in an OpenList that stores a list of vertices that have not been analyzed.

27. The computer readable medium of claim 23, wherein the plurality of next vertices are stored in a priority queue.

28. The computer readable medium of claim 27, wherein the plurality of next vertices stored in the priority queue are selected from a plurality of vertices stored in an OpenList that stores a list of vertices that have not been analyzed.

29. The computer readable medium of claim 23, wherein the respective priority of each of the plurality of next vertices is based on a distance from a starting position of a respective one of the plurality of machines to the respective one of the plurality of next vertices.

30. The computer readable medium of claim 29, wherein the respective priority of each of the plurality of next vertices is further based on an estimated distance from the respective one of the plurality of next vertices to an end position of the respective one of the plurality of machines.

31. The computer readable medium of claim 23 , wherein the determined path is complete when the second vertex is an end position of a respective one of the plurality of machines.

32. The computer readable medium of claim 31, wherein the completed path is compared to other paths for the respective one of the plurality of machines to determine a shortest path from a starting position to an end position of the respective one of the plurality of machines.

33. The computer readable medium of claim 23, wherein the second vertex is selected based whether there is an obstacle blocking a path for the one of the plurality of machines to the second vertex.