Method and apparatus for automatic vehicle guidance using continuous 2-D poly-point path

• US 8,131,415 B2
• Filed: 04/07/2008
• Issued: 03/06/2012
• Est. Priority Date: 05/24/2005
• Status: Active Grant

• Current Assignee: Trimble Navigation Limited (Trimble Inc.)
• Original Assignee: Trimble Navigation Limited (Trimble Inc.)
• Inventors: Peake, John W.
• Primary Examiner(s): Baeulieu, Yonel
• Application Number:
  US12/080,975
• Publication Number:
  US 20080275602A1
• Time in Patent Office: 1,429 Days
• Field of Search: 701/200, 701 23- 25
• US Class Current: 701/25
• CPC Class Codes: G05D 1/0212 with means for defining a desired trajectory involving a plurality of land vehicles G05D1/0287

Abstract

A method for using an arbitrary shape 2-D poly-point path for an automatic vehicle guidance comprising the steps of: (A) determining a set of 2-D reference points; (B) selecting a set of 2-D interpolating curves to fit the set of 2-D reference points, wherein each 2-D interpolation curve connects a pair of 2-D reference points, and wherein each set of 2-D interpolation curves comprises a continuous 2-D poly-point path; and (C) calculating a steering angle to guide the vehicle along the continuous 2-D poly-point path.

Claims

1. A method of automatic vehicle guidance comprising:

(A) determining a set of 2-D reference points;

(B) selecting a set of 2-D interpolating curves to fit said set of 2-D reference points, each said 2-D interpolation curve connecting a pair of said 2-D reference points;

said set of 2-D interpolation curves comprising a continuous 2-D poly-point path;

(C) calculating a steering angle;

and

(D) performing an automatic guidance of said vehicle along said continuous 2-D poly-point path.

2. The method of claim 1, wherein said step (A) further comprises:

(A1) using a master line to supply said set of 2-D reference points.

3. The method of claim 1, wherein said step (A) further comprises:

(A2) using a Geographical Information System (GIS) to determine said set of 2-D reference points.

4. The method of claim 1, wherein said step (A) further comprises:

(A3) navigating a vehicle to collect said set of 2-D reference points;

said vehicle including a navigational processor and a memory block; and

wherein a maximum number of said set of 2-D reference points is limited by said memory block and by an operational speed of said navigational processor.

5. The method of claim 1, wherein said step (B) of selecting said set of interpolation curves to fit said set of 2-D reference points further comprises:

(B1) selecting said set of 2-D interpolation curves based on a set of criteria selected from the group consisting of;

continuity of path;

continuity of heading;

and continuity of radius of curvature.

6. The method of claim 5, wherein said step (B1) further comprises:

(B1,

     1) calculating a set of 2-D parameterized cubic spline functions.

7. The method of claim 1, wherein said step (C) further comprises:

(C1) determining said vehicle position at a guidance point;

(C2) calculating a closest point to said vehicle position at said guidance point on a spline path;

wherein said spline path is selected to interpolate said 2-D poly-point path;

(C3) calculating a cross track error, a heading error, and a path heading rate for said vehicle position at said guidance point;

(C4) using said cross track error and said heading error in a closed loop feedback to calculate a main component of a steering angle;

and

(C5) using an inverse kinematic model of said vehicle to translate said path heading rate into an additional component of said steering angle, wherein said additional component of said steering angle is configured to compensate for said spline path curvature.

8. The method of claim 7, wherein said step (C1) further comprises:

(C1,

     1) using a source of position measurements to determine said vehicle position at said guidance point;

said source of position measurement is selected from the group consisting of;

a carrier differential SATPS;

a code differential SATPS;

an autonomous SATPS;

an Inertial Navigation System (INS); and

a Pseudolite-Based Positioning System.

9. A method of automatic vehicle guidance comprising:

(A) determining an initial set of 2-D reference points in a sliding spline fit window;

(B) calculating a set of 2-D parameterized cubic spline functions to fit said initial set of 2-D reference points located inside said sliding spline fit window;

(C) selecting a guidance window located inside said sliding spline fit window;

(D) guiding said vehicle along a continuous 2-D poly-point path by calculating a steering angle while said vehicle is located inside said guidance window;

and

(E) if the guidance position of said vehicle is moved outside said guidance window, repeating said steps (A-D).

10. The method of claim 9, wherein said step (C) of selecting said guidance window located inside said sliding spline fit window further comprises:

(C1) selecting a subset of substantially precise 2-D parameterized cubic spline functions configured to substantially precisely fit a subset of 2-D reference points located inside said guidance window, said subset of 2-D substantially precise parameterized cubic spline functions having substantially precise coefficients for guidance said vehicle located inside said guidance window.

11. An apparatus for automatic vehicle guidance by using an arbitrary shape 2-D poly-point path, said apparatus comprising:

(A) a processor configured to determine a set of 2-D reference points;

(B) a processor configured to select a set of 2-D interpolating curves to fit said set of 2-D reference points, each said 2-D interpolation curve connecting a pair of said 2-D reference points;

said set of 2-D interpolation curves comprising a continuous 2-D poly-point path;

and

(C) a processor configured to guide said vehicle by calculating a steering angle for said vehicle along said continuous 2-D poly-point path.

12. The apparatus of claim 11, wherein said processor (A) further comprises:

(A1) a processor configured to use a given master line to determine said set of 2-D reference points.

13. The apparatus of claim 11, wherein said means processor (A) further comprises:

(A2) a GIS configured to determine said set of 2-D reference points.

14. The apparatus of claim 11, wherein said processor (A) further comprises:

(A3) a processor configured to collect said set of 2-D reference points by navigating a vehicle;

said vehicle including a navigational processor and a memory block; and

wherein a maximum number of said set of 2-D reference points is limited by said memory block and by an operational speed of said navigational processor.

15. The apparatus of claim 11, wherein said processor (B) further comprises:

(B1) a processor configured to select said set of 2-D interpolation curves based on a set of criteria selected from the group consisting of;

continuity of path;

continuity of heading; and

continuity of radius of curvature.

16. The apparatus of claim 15, wherein said means (B1) further comprises:

(B1,

     1) a processor configured to calculate said set of 2-D parameterized cubic spline functions.

17. The apparatus of claim 11, wherein said processor (C) further comprises:

(C1);

a source of position measurements configured to determine said vehicle position at said guidance point;

said source of position measurement is selected from the group consisting of;

a carrier differential SATPS;

a code differential SATPS;

an autonomous SATPS;

an Inertial Navigation System (INS); and

a Pseudolite-Based Positioning System;

(C2) a processor configured to calculate a closest point to said vehicle position at said guidance point on a spline path;

wherein said spline path is selected to interpolate said 2-D poly-point path;

(C3) a processor configured to calculate a cross track error, a heading error, and a path heading rate for said vehicle position at said guidance point;

(C4) a processor configured to calculate a main component of a steering angle;

and

(C5) a processor configured to translate said path heading rate into an additional component of said steering angle, wherein said additional component of said steering angle is configured to compensate for said spline path curvature.

18. The apparatus of claim 17, wherein said means (C1) further comprises:

(C1,

     1) said source of position measurement selected from the group consisting of;

a carrier differential SATPS;

a code differential SATPS;

an autonomous SATPS;

an Inertial Navigation System (INS); and

a Pseudolite-Based Positioning System.

19. The apparatus of claim 17, wherein said processor (C4) further comprises:

(C4,

     1) a closed loop feedback model configured to calculate a main component of a steering angle by using said cross track error and said heading error.

20. The apparatus of claim 17, wherein said processor (C5) further comprises:

(C5,

     1) an inverse kinematic model of said vehicle configured to translate said path heading rate into an additional component of said steering angle, wherein said additional component of said steering angle is configured to compensate for said spline path curvature.

21. An apparatus for automatic vehicle guidance by using an arbitrary shape 2-D poly-point path, said apparatus comprising:

a processor configured to determine an initial set of 2-D reference points in a sliding spline fit window;

a processor configured to calculate a set of 2-D parameterized cubic spline functions to fit said initial set of 2-D reference points located inside said sliding spline fit window;

a processor configured to select a guidance window located inside said sliding spline fit window;

and

a processor configured to guide said vehicle along a continuous 2-D poly-point path by calculating a steering angle while said vehicle is located inside said guidance window.

22. The apparatus of claim 21, wherein said processor configured to select said guidance window located inside said sliding spline fit window further comprises:

a processor configured to select a subset of substantially precise 2-D parameterized cubic spline functions configured to substantially precisely fit a subset of 2-D reference points located inside said guidance window, said subset of 2-D substantially precise parameterized cubic spline functions having substantially precise coefficients configured for guidance said vehicle located inside said guidance window.

23. The apparatus of claim 21, wherein said processor configured to select said guidance window located inside said sliding spline fit window further comprises:

a processor configured to select a subset of substantially precise 2-D parameterized cubic spline functions configured for guidance said vehicle located inside said guidance window.