Path planning autopilot

• US 9,020,757 B2
• Filed: 05/11/2012
• Issued: 04/28/2015
• Est. Priority Date: 05/11/2012
• Status: Active Grant

• Current Assignee: PTx Trimble LLC
• Original Assignee: Trimble Navigation Limited (Trimble Inc.)
• Inventors: Peake, John W, Pleines, Stephan
• Primary Examiner(s): Olszewski, John R
• Assistant Examiner(s): Do, Truc M
• Application Number:
  US13/470,028
• Publication Number:
  US 20130304300A1
• Time in Patent Office: 1,082 Days
• Field of Search: 701/50, 701/41, 701/23, 701/24, 701/25, 701/26, 701/400, 701/409, 701/448, 180/68, 180/204, 180/443, 180/444
• US Class Current: 701/500
• CPC Class Codes: A01B 69/008 automatic
• CPC Class Codes: B62D 15/025 Active steering aids, e.g. helping the driver by actively influencing the steering system after environment evaluation B62D1/28 takes precedence
• CPC Class Codes: B62D 6/00 Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits means for initiating a change in direction B62D1/00; steering valves B62D5/06; combined with means for inclining the vehicle body or wheels on bends B62D9/00
• CPC Class Codes: G05D 1/0217 in accordance with energy consumption, time reduction or distance reduction criteria
• CPC Class Codes: G05D 1/0257 using a radar radar systems designed for anti-collision purposes between land vehicles or between land vehicle and fixed obstacles G01S13/931
• CPC Class Codes: G05D 1/0278 using satellite positioning signals, e.g. GPS

Abstract

A path planning autopilot guides vehicles efficiently even when they are far from a desired path.

Claims

1. A method for guiding a vehicle to a desired path comprising:

creating a joining path from an initial vehicle configuration to the desired path, the joining path comprising a first optimal turn; and

,

using an autopilot to steer the vehicle along the joining path.

2. The method of claim 1, the joining path further comprising a line preceding the first optimal turn.

3. The method of claim 1, the joining path further comprising a second optimal turn following the first optimal turn.

4. The method of claim 1, the joining path further comprising a line following the first optimal turn and a second optimal turn following the line.

5. The method of claim 1, the first optimal turn comprising a first clothoid.

6. The method of claim 5, the first optimal turn further comprising a circular arc preceding the first clothoid.

7. The method of claim 5, the first optimal turn further comprising a second clothoid following the first clothoid.

8. The method of claim 5, the first optimal turn further comprising a circular arc following the first clothoid and a second clothoid following the circular arc.

9. The method of claim 1, the desired path being a line.

10. The method of claim 9, the joining path not exceeding a maximum angle φ

_max away from the line.

11. The method of claim 1, the joining path not exceeding a maximum curvature θ

_max.

12. The method of claim 1, the joining path not exceeding a maximum curvature rate μ

.

13. The method of claim 1 wherein creating a joining path comprises:

calculating a distance y(φ

₀, θ

₀) traversed perpendicular to the desired path during an optimal turn from the initial vehicle configuration to a configuration having a heading parallel to the desired path and zero curvature, where φ

₀ and θ

₀ are the vehicle'"'"'s heading and curvature in the initial vehicle configuration; and

,

calculating the first optimal turn from the initial vehicle configuration to the desired path if the vehicle'"'"'s cross track error relative to the desired path in the initial vehicle configuration equals y(φ

₀,θ

₀).

14. The method of claim 13 further comprising preceding the first optimal turn by a line heading φ

₀ if φ

₀ equals a maximum approach angle φ

_max and θ

₀=0.

15. The method of claim 1 wherein creating a joining path comprises:

calculating the first optimal turn from the initial vehicle configuration to a line inclined with respect to the desired path by a maximum angle φ

_max;

continuing the line until reaching a distance y(φ

,θ

) traversed perpendicular to the desired path during an optimal turn to a configuration having a heading parallel to the desired path and zero curvature, where φ and

θ

are the vehicle'"'"'s heading and curvature; and

,

calculating a second optimal turn from the line to the desired path.

16. The method of claim 1 wherein creating a joining path comprises:

calculating the first optimal turn from the initial vehicle configuration to a line inclined with respect to the desired path by an angle φ

_max; and

,

calculating a second optimal turn from a point on the first optimal turn to the desired path.

17. The method of claim 16, the point being y(φ

,θ

) away from the desired path, where y(φ

,θ

) is a distance traversed perpendicular to the desired path during an optimal turn to a configuration having a heading parallel to the desired path and zero curvature, where φ and

θ

are the vehicle'"'"'s heading and curvature.

18. The method of claim 1 further comprising:

creating a new joining path from a current vehicle configuration to the desired path whenever vehicle speed changes.

19. The method of claim 1 further comprising:

creating a new joining path from a current vehicle configuration to the desired path whenever a maximum allowable heading φ

_max changes.

20. The method of claim 1 further comprising:

creating a new joining path from a current vehicle configuration to the desired path whenever a maximum allowable curvature θ

_max changes.

21. The method of claim 1 further comprising:

creating a new joining path from a current vehicle configuration to the desired path whenever a maximum allowable curvature rate μ

changes.

22. The method of claim 1 further comprising:

creating a new joining path from a current vehicle configuration to the desired path at regular intervals.

23. The method of claim 1, the autopilot comprising a feedback controller and a feed-forward, path joining planner.

24. The method of claim 1, the autopilot comprising a GNSS receiver.

25. The method of claim 1, the autopilot comprising a vehicle heading sensor.

26. The method of claim 1, the autopilot comprising a vehicle steering angle sensor.

27. The method of claim 1, the autopilot comprising a vehicle steering actuator.

28. The method of claim 1, the autopilot comprising a microprocessor and memory.

29. The method of claim 1, the initial vehicle configuration including an initial heading and approximately zero curvature, the desired path being a line parallel to the initial heading but having a heading 180 degrees away, and the joining path being constructed so that it remains within a boundary perpendicular to the desired path.

30. A method for guiding a vehicle to a desired path comprising:

creating a joining path from an initial vehicle configuration to the desired path, the joining path comprising a first turn in a first direction and a second turn in a second direction opposite to the first; and

,

using an autopilot to steer the vehicle along the joining path.

31. The method of claim 30, the desired path being a line.

32. The method of claim 31, the joining path not exceeding a maximum angle φ

_max away from the line.

33. The method of claim 30, the joining path not exceeding a maximum curvature θ

_max.

34. The method of claim 30, the joining path not exceeding a maximum curvature rate μ

.

35. The method of claim 30, the joining path further comprising a line.

36. The method of claim 30, the joining path having an end at which the joining path and the desired path have the same heading and curvature, and cross track error between the two paths is minimized.

37. The method of claim 30, the first turn comprising a clothoid.

38. The method of claim 30, the second turn comprising a clothoid.

39. A method for guiding a vehicle to a desired path comprising:

(a) creating a first joining path from an initial vehicle configuration to a line tangent to the desired path, the line being tangent at a point on the desired path nearest to the initial vehicle configuration, the joining path comprising a first optimal turn;

(b) repeating step (a) from a point on the first joining path to build an overall continuous curvature joining path that ends close enough to the desired path that feedback techniques may be used to guide the vehicle from the end of the overall joining path the desired path; and

,

(c) using an autopilot to steer the vehicle along the joining path.