System and method for autonomous navigation of a tracked or skid-steer vehicle

• US 8,612,084 B2
• Filed: 09/15/2010
• Issued: 12/17/2013
• Est. Priority Date: 09/15/2009
• Status: Active Grant

• Current Assignee: Technological Resources Pty. Ltd. (Rio Tinto Limited)
• Original Assignee: University of Sydney
• Inventors: Hennessy, Ross, Oppolzer, Florian, Fan, Xiuyi, Durrant-Whyte, Hugh, Singh, Surya P. N.
• Primary Examiner(s): Marc, McDieunel
• Application Number:
  US13/496,481
• Publication Number:
  US 20120179322A1
• Time in Patent Office: 1,189 Days
• Field of Search: 701/3, 701/14, 701/23, 701/36, 701/24, 701/117, 701/120, 701/301, 701/410, 701/436, 701/467, 701/468, 701/489, 701/520, 701/532, 701/533, 701/538, 340/961, 340/988, 340/990, 340/995.17, 340/995.24, 340/995.23, 340/995.27, 340/357.29, 340/357.46, 342/36, 342/357.31, 342/451, 342/456, 342/455, 342/457, 318/586, 318/587, 708/442, 901/1, 244/75.1
• US Class Current: 701/25
• CPC Class Codes: G05D 1/027 comprising intertial navigation means, e.g. azimuth detector inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00
• CPC Class Codes: G05D 1/0278 using satellite positioning signals, e.g. GPS

Abstract

An autonomous navigation system for a tracked or skid-steer vehicle is described. The system includes a path planner (54) that computes a series of waypoint locations specifying a path to follow and vehicle location sensors (82). A tramming controller (60) includes a waypoint controller (62) that computes vehicle speed and yaw rate setpoints based on vehicle location information from the vehicle location sensor and the locations of a plurality of neighboring waypoints, and a rate controller (64) that generates left and right track speed setpoints from the speed and yaw rate setpoints. A vehicle control interface actuates the vehicle controls in accordance with the left and right track speed setpoints.

Claims

1. An autonomous navigation system for a tracked or skid-steer vehicle comprising:

a path planner that computes a series of waypoint locations specifying a path to follow;

a vehicle location sensor;

a tramming controller including a waypoint controller that computes vehicle speed and yaw rate setpoints based on vehicle location information from the vehicle location sensor and the locations of a plurality of neighbouring waypoints, and a rate controller that generates left and right track speed setpoints from the speed and yaw rate setpoints; and

a vehicle control interface that actuates vehicle controls in accordance with the left and right track speed setpoints.

2. An autonomous navigation system according to claim 1, wherein the waypoint controller uses a calculation of trajectory curvature determined at the waypoint nearest to the vehicle for computing the vehicle speed setpoint.

3. An autonomous navigation system according to claim 2 wherein, in computing the vehicle speed setpoint the waypoint controller also uses a measurement of the vehicle speed and yaw rate, and the maximum allowed vehicle speed and yaw rate.

4. An autonomous navigation system according to claim 1 wherein, in computing the vehicle yaw rate setpoint, the waypoint controller calculates a vehicle heading error defined by the angular difference between the vehicle heading and a vector dependent on the vehicle position, the current tracking waypoint and the following waypoint.

5. An autonomous navigation system according to claim 4 wherein, in computing the vehicle yaw rate setpoint, the waypoint controller calculates a waypoint heading error defined by the angular difference between the vehicle heading and the desired heading at the current tracking waypoint, wherein the desired heading at a waypoint is defined as the vector that connects the waypoint and its successive waypoint.

6. An autonomous navigation system according to claim 5 wherein, in computing the vehicle yaw rate setpoint, the waypoint controller calculates a cross-track error defined by the distance between the vehicle position and the planned path.

7. An autonomous navigation system according to claim 6, wherein the cross-track error corresponds to the distance between the vehicle position and the nearest point from a fine discretization of an interpolated path through the waypoints.

8. An autonomous navigation system according to claim 6, wherein the waypoint controller computes the vehicle yaw rate setpoint on the basis of a weighted combination of the vehicle heading error, the waypoint heading error, and the cross-track error.

9. An autonomous navigation system according claim 1 implemented on a surface drilling rig for drilling blast hole patterns in mining operations.

10. An autonomous navigation system according to claim 1, including a vehicle location sequencer that determines an ordered sequence of desired vehicle locations, and wherein the waypoints computed by the path planner specify a path between the desired vehicle locations.

11. An autonomous navigation system according to claim 10 wherein the path planner determines a path between desired vehicle locations using a cubic Bezier spline satisfying one or more predetermined path constraints including at least one minimum turning radius, obstacles and/or boundaries.

12. An autonomous navigation system according to claim 11 wherein the path planner includes a state lattice search system operating with a set of primitive expansion paths such that, if a valid path from one location to the next cannot be determined, the path planner expands its search space using the set of primitive expansion paths to a plurality of nodes defining nearby reachable vehicle configurations.

13. An autonomous navigation system according to claim 12 wherein the set of primitive expansion paths are generated using cubic Bezier splines.

14. An autonomous navigation system according to claim 13 wherein the path planner also implements a multidimensional occupancy grid representation for path verification.

15. A method for autonomous navigation of a tracked or skid-steer vehicle, comprising:

determining by a processor a path plan including a series of computed waypoint locations specifying a path to follow;

measuring the vehicle location and velocity;

computing by the processor vehicle speed and yaw rate setpoints based on the measured vehicle location and velocity and the locations of a plurality of neighbouring waypoints;

generating left and right track speed setpoints from the speed and yaw rate setpoints; and

controlling the vehicle left and right tracks in accordance with the left and right track speed setpoints.

16. A method for autonomous vehicle navigation according to claim 15 wherein computation of the vehicle speed setpoint is based on a calculation of trajectory curvature determined at the waypoint nearest to the vehicle.

17. A method for autonomous vehicle navigation according to claim 16 wherein computation of the vehicle speed setpoint also uses a measurement of the vehicle speed and yaw rate, and the maximum allowed vehicle speed and yaw rate.

18. A method for autonomous vehicle navigation according to claim 15 wherein computation of the vehicle yaw rate setpoint is based on a vehicle heading error defined by the angular difference between the vehicle heading and a weighted average vector dependent on the vehicle position, the current tracking waypoint and the following waypoint.

19. A method for autonomous vehicle navigation according to claim 18 wherein computation of the vehicle yaw rate setpoint is also based on a waypoint heading error defined by the angular difference between the vehicle heading and the desired heading at the current tracking waypoint, wherein the desired heading at a waypoint is defined as the vector that connects the waypoint and its successive waypoint.

20. A method for autonomous vehicle navigation according to claim 19 wherein computation of the vehicle yaw rate setpoint is also based on a cross-track error defined by the distance between the vehicle position and the planned path.

21. A method for autonomous vehicle navigation according to claim 20 wherein the cross-track error is defined by the distance between the vehicle position and the nearest point from a fine discretization of an interpolated path through the waypoints.

22. A method for autonomous vehicle navigation according to claim 20 wherein the vehicle yaw rate setpoint is computed on the basis of a weighted combination of the vehicle heading error, the waypoint heading error, and the cross-track error.

23. A method for autonomous vehicle navigation according to claim 15 implemented on a surface drilling rig for drilling blast hole patterns in mining operations.

24. A method for autonomous vehicle navigation according to claim 15 including a vehicle location sequencing step that determines an ordered sequence of desired vehicle locations, wherein the waypoints computed in the path plan specify a path between desired vehicle locations.

25. A method for autonomous vehicle navigation according to claim 24 including determining a path between desired vehicle locations using a cubic Bezier spline satisfying one or more path constraints including at least one of minimum turning radius, obstacles and/or boundaries.

26. A method for autonomous vehicle navigation according to claim 25 including a state lattice search using a set of primitive expansion paths such that, if a valid path from one location to the next cannot be determined, the search space is expanded using the set of primitive expansion paths to a plurality of nodes defining nearby reachable vehicle configurations.

27. A method for autonomous vehicle navigation according to claim 26 wherein the set of primitive expansion paths are generated using cubic Bezier splines.

28. A method for autonomous vehicle navigation according to claim 27 wherein determining a path plan also includes use of a multidimensional occupancy grid representation for path verification.

29. A path planning system for autonomous vehicle having actuators for moving and steering the vehicle, the system comprising:

a path planner that receives a series of desired locations and generates path data defining a feasible path for the vehicle to traverse the series of locations;

sensors that monitor a location and heading of the vehicle;

a controller that receives the path data and the monitored location and heading of vehicle and determines setpoints for the actuators such that the vehicle follows the path.

30. A path planning system according to claim 29 wherein the path data comprises a series of waypoints and wherein the controller determines a desired vehicle heading that is a weighted average of:

a first angle between the vehicle heading and a first vector defined by the vehicle location and a current waypoint; and

a second angle between the vehicle heading and a second vector defined by the vehicle location and a subsequent waypoint in the series of waypoints.

31. A path planning system according to claim 30 wherein the weighted average depends on a distance between a current vehicle location and an earlier waypoint in the series of waypoints.

32. A path planning system according to claim 31 wherein a weighting of the second angle increases as the distance increases.

33. A method for planning a path for an autonomous vehicle having defined constraints, the method comprising:

receiving by a processor a starting location and an end location;

generating by the processor a path that connects the starting and the end locations;

if the path does not satisfy the defined constraints, applying a state lattice search in which a search space is expanded using a set of primitive expansion paths to a plurality of nodes defining nearby reachable vehicle configurations.