Modular path planner

• US 7,110,881 B2
• Filed: 10/07/2003
• Issued: 09/19/2006
• Est. Priority Date: 10/07/2003
• Status: Active Grant

• Current Assignee: Deere & Company
• Original Assignee: Deere & Company
• Inventors: Hansen, Shane Lynn, Gray, Sarah Ann, Flann, Nicholas Simon
• Primary Examiner(s): Chin, Gary
• Application Number:
  US10/680,307
• Publication Number:
  US 20050075784A1
• Time in Patent Office: 1,078 Days
• Field of Search: 701/23, 701/25, 701/50, 701/200, 701/210, 701/202, 701/209, 340/995.19, 340/995.21
• US Class Current: 701/209
• CPC Class Codes: A01B 69/008 automatic
• CPC Class Codes: G01C 21/3867 Geometry of map features, e.g. shape points, polygons or for simplified maps
• CPC Class Codes: G05D 1/0212 with means for defining a desired trajectory involving a plurality of land vehicles G05D1/0287

Abstract

A perimeter training module establishes a perimeter path plan of a vehicle including a defined perimeter. A region-filling module establishes a region-filling path plan of the vehicle within the defined perimeter. A point-to-point planning module establishes a point-to-point path plan of a vehicle including a segment of at least one of the region-filling path plan and the perimeter path plan. The path planner forms a preferential composite path plan based on the established perimeter path plan, the region-filling plan, and the point-to-point path plan.

Claims

1. A method for planning a path of a vehicle, the method comprising:

establishing a perimeter path plan of the vehicle including a defined perimeter, the perimeter path plan comprising a least one segment;

establishing a region-filling path plan of the vehicle within the defined perimeter, the region-filling path plan comprising a series of interconnected segments;

establishing a point-to-point path plan of the vehicle including at least one of the segments of at least one of the region-filling path plan and the perimeter path plan; and

forming a preferential composite path plan comprising a list or ordered assembly of the segments by estimating candidate path distances of corresponding candidate composite paths and selecting a shortest candidate path as the preferential composite path based on the established perimeter path plan, the region-filling plan, and the point-to-point path plan, each segment being curved or straight and defined by a start point, an end point, and an arc radius.

2. The method according to claim 1 wherein the forming comprises:

selecting preferential components of two or more of the established plans to form the preferential composite plan consistent with minimizing an economic cost of traversing the composite path plan by the vehicle.

3. The method according to claim 1 wherein the forming comprises:

estimating candidate path distances of corresponding candidate composite path plans, where each candidate composite path plan comprises a list of the segments for evaluation;

selecting the preferential composite path plan as the candidate composite path plan having a shortest estimated distance; and

defining the preferential composite path plan as components of two or more of the established plans.

4. The method according to claim 1 wherein the preferential composite path plan comprises an assembly of segments selected from at least two of the established perimeter plan, the established point-to-point path plan, and the established region-filling path plan;

an order of the assembly based on coordination that minimizes an overall composite economic cost of the preferential composite path plan.

5. The method according to claim 1 wherein the established perimeter path plan comprises a series of interconnected segments;

the established point-to-point path plan comprises a series of interconnected segments;

the established region-filling path plan comprises a series of interconnected segments, each of the interconnected segments selected from the group consisting of a generally linear segment and an arc.

6. The method according to claim 1 wherein the establishing of the perimeter path plan comprises:

defining vehicular constraints of the vehicle that relate to navigation of the vehicle;

collecting location data points by traversing a perimeter of a work area to be covered by the vehicle;

filtering the collected data to provide filtered data of critical data points to reduce high frequency noise in the collected data; and

selecting a path for the vehicle that is composed of segments that intercept the critical data points.

7. The method according to claim 6 wherein the segments that intercept the critical data points comprise tangential arcs.

8. The method according to claim 6 wherein the filtering further comprises:

dividing the collected data into at least one of a temporal sequence and a spatial sequence indicative of vehicular position with reference to a first axis and a second axis, the first axis being generally perpendicular to the second axis;

accessing pre-filtered magnitude data of the collected data to determine critical magnitude data;

applying a zero-phase filtering to the collected data to determine critical phase data;

identifying the critical data points based on the critical magnitude data and the critical phase data.

9. The method according to claim 6 wherein the filter further comprises:

calculating a cutoff frequency of a filter based on at least one of a maximum velocity of the vehicle during the collecting step, minimum turn radius, duration between samples collected during the collecting step, and a damping factor for the filter;

calculating filter coefficients for the filter.

10. The method according to claim 9 wherein the filter comprises a Butterworth filter.

11. The method according to claim 6 wherein the selection further comprises:

calculating a path angle at each point in the path;

looping through the critical points while skipping candidate points;

determining a distance from a candidate point to an arc;

designating the candidate point as a disregarded point if the determined distance is greater than a threshold.

12. The method according to claim 11 wherein the selection further comprises:

designating the candidate point as a critical point if the determined distance is less than or equal to the threshold and revising the looping to include the newly designated critical point.

13. The method according to claim 1 wherein said establishing the point-to-point path plan comprises:

establishing a minimum turning radius of a vehicle;

defining a starting point and starting heading of the vehicle, the starting point lying within an operational zone of the vehicle;

defining a destination point and destination heading of the vehicle, the destination point lying with the operation zone;

scanning the operational zone between the starting point and the destination point to identify any obstacle and a velocity vector associated with the obstacle; and

determining a point-to-point path between the starting point and the destination point consistent with avoidance of a collision with the obstacle and the established minimum turning radius.

14. The method according to claim 13 further comprising:

defining the obstacle, as a stationary object, within the operational zone between the starting point and destination point.

15. The method according to claim 13 wherein the obstacle is defined as a polygonal shape in which the vehicle must keep out in accordance with a navigational rule.

16. The method according to claim 13 wherein the point-to-point path is determined by identifying a list of candidate paths and searching the candidate paths for a preferential path with a minimal economic cost of traversal.

17. The method according to claim 16 wherein the preferential path comprises the shortest path between the starting point and the destination point that avoids intersecting with any obstacle by a desired margin of safety.

18. The method according to claim 16 wherein the searching further comprises:

constructing a visibility graph of edges and nodes;

searching the visibility graph with an A* algorithm;

applying at least one of a straight line cost filter and a maximum allowed touch filter to eliminate solutions of the A* algorithm to enhance efficiency of the searching.

19. A method for establishing a point-to-point path plan, the method comprising:

establishing a minimum turning radius of a vehicle;

defining a starting point and starting heading of the vehicle, the starting point lying within an operational zone of the vehicle;

defining a destination point and destination heading of the vehicle, the destination point lying with the operation zone;

scanning the operational zone between the starting point and the destination point to identify any obstacle and a velocity vector associated with the obstacle; and

determining a path between the starting point and the destination point consistent with avoidance of a collision with the obstacle and the established minimum turning radius.

20. The method according to claim 19 further comprising:

defining an obstacle, as a stationary object, within the operation zone between the starting point and destination point.

21. The method according to claim 19 wherein the obstacle is defined as a polygonal shape in which the vehicle must keep out in accordance with a navigational rule.

22. The method according to claim 19 wherein the path is determined by identifying a list of candidate paths and searching the candidate paths for a preferential path with a minimal economic cost of traversal.

23. The method according to claim 22 wherein the preferential path comprises the shortest path between the starting point and the destination point that avoids intersecting with any obstacle by a desired margin of safety.

24. The method according to claim 19 wherein the searching further comprises:

constructing a visibility graph of edges and nodes;

searching the visibility graph with an A* algorithm;

applying at least one of a straight line cost filter and a maximum allowed touch filter to eliminate solutions of the A* algorithm to enhance efficiency of the searching.

25. A system for planning a path of a vehicle, the system comprising:

a perimeter training module for establishing a perimeter path plan of the vehicle including a defined perimeter, the perimeter path plan comprising a least one segment;

a region-filling module for establishing a region-filling path plan of the vehicle within the defined perimeter, the region-filling path plan comprising a series of interconnected segments;

a point-to-point planning module for establishing a point-to-point path plan of the vehicle including at least one of the segments of at least one of the region-filling path plan and the perimeter path plan; and

a coordination module for forming a preferential composite path plan comprising a list or ordered assembly of the segments by estimating candidate path distances or corresponding candidate composite paths and selecting a shortest candidate path as the preferential composite path based on the established perimeter path plan, the region-filling plan, and the point-to-point path plan, each segment being curved or straight and defined by a start point, an end point, and an arc radius.

26. The system according to claim 25 wherein the coordination module comprises preferential components of two or more of the established plans to form the preferential composite plan consistent with minimizing an economic cost of traversing the composite path plan by the vehicle.

27. The system according to claim 25 wherein the coordination module comprises:

an estimator for estimating candidate path distances of corresponding candidate composite path plans, where each candidate composite path plan comprises a list of the segments for evaluation;

a selector for selecting the preferential composite path plan as the candidate composite path plan having a shortest estimated distance; and

a definer for defining the preferential composite path plan as components of two or more of the established plans.

28. The system according to claim 25 wherein the preferential composite path plan comprises an assembly of segments selected from at least two of the established perimeter plan, the established point-to-point path plan, and the established region-filling path plan;

an order of the assembly based on coordination that minimizes an overall composite economic cost of the preferential composite path plan.