System and method for controlling a vehicle

• US 9,746,854 B2
• Filed: 04/25/2016
• Issued: 08/29/2017
• Est. Priority Date: 04/24/2015
• Status: Active Grant

• Current Assignee: Autonomous Solutions, Inc.
• Original Assignee: Autonomous Solutions, Inc.
• Inventors: Berkemeier, Matthew D.
• Primary Examiner(s): PATEL, SHARDUL D
• Application Number:
  US15/138,065
• Publication Number:
  US 20160313735A1
• Time in Patent Office: 491 Days
• Field of Search: 701 1, 701 25, 701412, 701418, 382291
• CPC Class Codes: A01B 69/008 automatic
• CPC Class Codes: A47L 11/282 having rotary tools
• CPC Class Codes: A47L 11/4011 Regulation of the cleaning machine by electric means; Control systems and remote control systems therefor
• CPC Class Codes: A47L 11/4044 Vacuuming or pick-up tools; Squeegees
• CPC Class Codes: A47L 11/4061 Steering means; Means for avoiding obstacles; Details related to the place where the driver is accommodated
• CPC Class Codes: A47L 11/4066 Propulsion of the whole machine
• CPC Class Codes: A47L 11/4072 Arrangement of castors or wheels
• CPC Class Codes: A47L 2201/00 Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation
• CPC Class Codes: A47L 2201/04 Automatic control of the travelling movement; Automatic obstacle detection
• CPC Class Codes: G05D 1/0212 with means for defining a desired trajectory involving a plurality of land vehicles G05D1/0287
• CPC Class Codes: G05D 1/0219 ensuring the processing of the whole working surface
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30

Abstract

Drivable path plan systems and methods for autonomous vehicles disclosed herein may receive original path plan data, including a first path element tangentially connected to a second path element at a transition connection point. A drivable path plan may be calculated for the autonomous vehicle between the first path element and the second path element using a clothoid spline. An initial connection point may be identified, as well as an initial heading and an initial curvature along the first path element, and a final connection point, a final heading, and a final curvature along the second path element. The clothoid spline may be inserted between the initial connection point along the first path element and the final connection point along the second path element.

Claims

1. A drivable path plan system for an autonomous vehicle, the drivable path plan system comprising:

an original path plan module configured to receive original path plan data indicative of an original path plan for an autonomous vehicle, the original path plan comprising;

a first path element comprising a first circular arc having a first circular arc curvature; and

a second path element tangentially connected to the first path element at a transition connection point;

a drivable path plan module configured to calculate a drivable path plan for the autonomous vehicle between the first path element and the second path element comprising a clothoid spline, the drivable path plan module comprising;

a connection point identification module configured to identify an initial connection point, an initial heading, and an initial curvature along the first path element, and a final connection point, a final heading, and a final curvature along the second path element; and

a clothoid spline calculation module configured to calculate the clothoid spline between the initial connection point on the first path element and the final connection point on the second path element, the clothoid spline comprising;

a first clothoid segment tangentially connected to the first path element at the initial connection point, the first clothoid segment having a first clothoid segment initial curvature and a first clothoid segment final curvature, wherein the first clothoid segment initial curvature is equal to the first circular arc curvature of the first path element at the initial connection point;

a second clothoid segment tangentially connected to the first clothoid segment at a second connection point, the second clothoid segment having a second clothoid segment initial curvature and a second clothoid segment final curvature, wherein the second clothoid segment initial curvature is equal to the first clothoid segment final curvature at the second connection point; and

a third clothoid segment tangentially connected to the second clothoid segment at a third connection point, the third clothoid segment having a third clothoid segment initial curvature and a third clothoid segment final curvature, wherein the third clothoid segment initial curvature is equal to the second clothoid segment final curvature at the third connection point, and wherein the third clothoid segment is tangentially connected to the second path element at the final connection point, wherein the third clothoid segment final curvature is equal to a second path element curvature at the final connection point.

2. The drivable path plan system of claim 1, wherein the second path element comprises a second circular arc having a second circular arc curvature.

3. The drivable path plan system of claim 1, wherein the second path element comprises a straight line.

4. The drivable path plan system of claim 1, wherein the clothoid spline is selected to minimize a lateral acceleration experienced by the autonomous vehicle as the autonomous vehicle executes the drivable path plan between the first path element and the second path element.

5. The drivable path plan system of claim 1, wherein the clothoid spline is selected to minimize an off-path error of the drivable path plan with respect to the original path plan.

6. The drivable path plan system of claim 5, wherein a magnitude of the clothoid spline corresponds to a speed and a constant maximum curvature rate of the autonomous vehicle.

7. The drivable path plan system of claim 6, wherein the autonomous vehicle comprises a rate-limited servo steering system having a maximum turning rate.

8. A method of calculating a drivable path plan for an autonomous vehicle, the method comprising:

at a processor configured to calculate a drivable path plan for an autonomous vehicle;

receiving original path plan data indicative of an original path plan for an autonomous vehicle, the original path plan comprising;

a first path element comprising a first circular arc having a first circular arc curvature; and

a second path element tangentially connected to the first path element at a transition connection point;

identifying an initial connection point, an initial heading, and an initial curvature along the first path element, and a final connection point, a final heading, and a final curvature along the second path element; and

calculating a drivable path plan for the autonomous vehicle between the first path element and the second path element, the drivable path plan comprising a clothoid spline, wherein the clothoid spline comprises;

a first clothoid segment tangentially connected to the first path element at the initial connection point, the first clothoid segment having a first clothoid segment initial curvature and a first clothoid segment final curvature, wherein the first clothoid segment initial curvature is equal to the first circular arc curvature of the first path element at the initial connection point;

a second clothoid segment tangentially connected to the first clothoid segment at a second connection point, the second clothoid segment having a second clothoid segment initial curvature and a second clothoid segment final curvature, wherein the second clothoid segment initial curvature is equal to the first clothoid segment final curvature at the second connection point; and

a third clothoid segment tangentially connected to the second clothoid segment at a third connection point, the third clothoid segment having a third clothoid segment initial curvature and a third clothoid segment final curvature, wherein the third clothoid segment initial curvature is equal to the second clothoid segment final curvature at the third connection point, and wherein the third clothoid segment is tangentially connected to the second path element at the final connection point, wherein the third clothoid segment final curvature is equal to a second path element curvature at the final connection point.

9. The method of claim 8, wherein the second path element comprises a second circular arc having a second circular arc curvature.

10. The method of claim 8, wherein the second path element comprises a straight line.

11. The method of claim 8, wherein the clothoid spline is selected to minimize a lateral acceleration experienced by the autonomous vehicle as the autonomous vehicle executes the drivable path plan between the first path element and the second path element.

12. The method of claim 8, wherein the clothoid spline is selected to minimize an off-path error of the drivable path plan with respect to the original path plan.

13. The method of claim 12, wherein a magnitude of the clothoid spline corresponds to a speed and a constant maximum curvature rate of the autonomous vehicle.

14. The method of claim 13, wherein the autonomous vehicle comprises a rate-limited servo steering system having a maximum turning rate.

15. A computer program product for calculating a drivable path plan for an autonomous vehicle, the computer program product comprising:

a nontransitory computer readable medium; and

computer program code, encoded on the nontransitory computer readable medium, configured to cause at least one processor to perform the steps of;

receiving original path plan data indicative of an original path plan for an autonomous vehicle, the original path plan comprising;

a first path element comprising a first circular arc having a first circular arc curvature; and

a second path element tangentially connected to the first path element at a transition connection point;

identifying an initial connection point, an initial heading, and an initial curvature along the first path element, and a final connection point, a final heading, and a final curvature along the second path element; and

calculating a drivable path plan for the autonomous vehicle between the first path element and the second path element, the drivable path plan comprising a clothoid spline, wherein the clothoid spline comprises;

a first clothoid segment tangentially connected to the first path element at the initial connection point, the first clothoid segment having a first clothoid segment initial curvature and a first clothoid segment final curvature, wherein the first clothoid segment initial curvature is equal to the first circular arc curvature of the first path element at the initial connection point;

a second clothoid segment tangentially connected to the first clothoid segment at a second connection point, the second clothoid segment having a second clothoid segment initial curvature and a second clothoid segment final curvature, wherein the second clothoid segment initial curvature is equal to the first clothoid segment final curvature at the second connection point; and

a third clothoid segment tangentially connected to the second clothoid segment at a third connection point, the third clothoid segment having a third clothoid segment initial curvature and a third clothoid segment final curvature, wherein the third clothoid segment initial curvature is equal to the second clothoid segment final curvature at the third connection point, and wherein the third clothoid segment is tangentially connected to the second path element at the final connection point, wherein the third clothoid segment final curvature is equal to a second path element curvature at the final connection point.

16. The computer program product of claim 15, wherein the second path element comprises a second circular arc having a second circular arc curvature.

17. The computer program product of claim 15, wherein the second path element comprises a straight line.

18. The computer program product of claim 15, wherein the clothoid spline is selected to minimize a lateral acceleration experienced by the autonomous vehicle as the autonomous vehicle executes the drivable path plan between the first path element and the second path element.

19. The computer program product of claim 15, wherein the clothoid spline is selected to minimize an off-path error of the drivable path plan with respect to the original path plan.

20. The computer program product of claim 19, wherein a magnitude of the clothoid spline corresponds to a speed and a constant maximum curvature rate of the autonomous vehicle.