Method and system for estimating navigability of terrain

• US 7,272,474 B1
• Filed: 03/31/2005
• Issued: 09/18/2007
• Est. Priority Date: 03/31/2004
• Status: Active Grant

• Current Assignee: Carnegie Mellon University
• Original Assignee: Carnegie Mellon University
• Inventors: Stentz, Anthony, Wellington, Carl Knox
• Primary Examiner(s): Zanelli; Michael J
• Application Number:
  US11/096,333
• Time in Patent Office: 901 Days
• Field of Search: None
• US Class Current: 701/26
• CPC Class Codes: G01C 21/08 involving use of the magnetic field of the earth
• CPC Class Codes: G01C 7/04 involving a vehicle which moves along the profile to be traced

Abstract

A method and system for detecting an obstacle comprises a terrain estimator for estimating a local terrain surface map based on at least one of range data points, color data, and infrared data gathered by electromagnetic perception focused in front of a vehicle. The map is composed of a series of terrain cells. An analyzer estimates at least one of predicted roll data, predicted pitch data, predicted ground clearance data, and predicted friction coefficient data based on the estimated terrain map for respective terrain cells and vehicular constrain data. A local planner determines predicted vehicle control data for terrain cells within the terrain along a planned path of the vehicle. One or more vehicle sensors sense at least one of actual roll data, actual pitch data, actual ground clearance data, and actual friction coefficient data for the terrain cells when the vehicle is coextensively positioned with the corresponding terrain cell. A learning module adjusts at least one of the terrain map estimation and the control data determination based on the sensed actual data.

Claims

1. A method for estimating navigability of terrain, the method comprising:

estimating a terrain map based on range data points gathered by electromagnetic perception from a vehicle, the map composed of a series of terrain cells;

estimating at least one of predicted roll angle data, predicted pitch angle data, and predicted ground clearance data based on the estimated terrain map for at least one respective particular terrain cell and vehicular constraint data;

determining at least one of vehicle control data and vehicle state data for terrain cells within the terrain along a planned path of the vehicle based on the estimations;

sensing at least one of actual roll angle data, actual pitch angle data, and actual ground clearance data for the at least one particular terrain cell when the vehicle is coextensively positioned within the corresponding particular terrain cell for which an estimation has been made; and

adjusting at least one of the terrain map, the vehicle control data, and vehicle state data based on the sensed actual data.

2. The method according to claim 1 wherein each terrain cell is associated with an average terrain height, a lowest point, and a deviation from a generally planar reference surface.

3. The method according to claim 1 wherein estimating of the roll angle data is estimated in accordance with the following equation:

roll=a sin [(z_RearLeft−

z_RearRight)/RearTrackWidth],

where roll is the roll angle, a sin is arc sin, z_RearLeft is the left rear wheel height, z_RearRight is right wheel height, and RearTrackWidth is the spacing between the left rear wheel and the right rear wheel of the vehicle.

4. The method according to claim 1 wherein estimating of the pitch angle data is estimated in accordance with the following equation:

pitch=a sin [(z_FrontCenter−

z_RearCenter)/WheelBase]

where pitch is the pitch angle, a sin is arc sin, z_FrontCenter is the height of the center for the front axle, wherein z_RearCenter is the height of the center for the rear axle or wheel bearing, and wherein the Wheelbase is the spacing between the front axle and the rear axle.

5. The method according to claim 1 wherein the estimating a terrain map comprises estimating the terrain map based on at least one of range data points, color data, and infrared data gathered by electromagnetic perception focused in front of a vehicle.

6. The method according to claim 1 wherein the estimating at least one of further comprises estimating predicted friction coefficient data based on the estimated terrain map for respective terrain cells and the vehicular constraint data.

7. The method according to claim 1 wherein the vehicular constraint data comprise one or more of the following:

physical specifications of the vehicle, dimensions, stability, roll-over resistance, ground clearance, turning radius, cruising speed, fuel capacity and maximum range of vehicle.

8. The method according to claim 1 wherein the adjusting of the vehicle control data comprises selecting a preferential local path plan to maintain stability of a vehicle within each terrain cell that the planned path intercepts.

9. The method according to claim 1 wherein the adjusting of the vehicle control data comprises avoiding vehicle entry into any terrain cells in which a maximum roll angle for the vehicle is predicted to be exceeded.

10. The method according to claim 1 wherein the adjusting of the vehicle control data comprises avoiding vehicle entry into any terrain cells in which a maximum pitch angle of the vehicle is predicted to be exceeded.

11. The method according to claim 1 wherein the adjusting of the vehicle control data comprises avoiding vehicle entry into any terrain cells in which a bottom of the vehicle is predicted to collide with any peaks of the ground within the terrain cell.

12. A method for estimating navigability of terrain, the method comprising:

estimating a terrain map of local elevation of load-bearing surface of terrain, with vegetation in over at least part of the load-bearing surface, based on at least one of range data points, color data, and infrared data gathered by electromagnetic perception from a vehicle, the map composed of a series of terrain cells;

estimating at least one of predicted roll data, predicted pitch data, and predicted ground clearance data based on the load-bearing surface for respective terrain cells and vehicular constraint data;

determining predicted vehicle control data for terrain cells within the terrain along a planned path of the vehicle based on the estimations;

sensing at least one of actual roll data, actual pitch data, and actual ground clearance data for the terrain cells when the vehicle is coextensively positioned with the corresponding terrain cell for which an estimation has been made; and

adjusting at least one of the estimation of the load-bearing surface and the vehicle control data based on the sensed actual data.

13. The method according to claim 12 wherein the estimating a terrain map comprises using locally weighted regression to establish a relationship between data point features detected by a range finder and the true ground height where the ground is covered with vegetation.

14. The method according to claim 13 further comprising providing a confidence indicator on the estimated local elevation of the load-bearing surface for one or more cells of the terrain.

15. The method according to claim 12 wherein for the range data points a higher density within a defined area about a target indicates a solid object or ground and a lower density about the target indicates the presence of vegetation.

16. The method according to claim 12 wherein the terrain map comprises a group of cells, wherein each cell is associated with an average terrain height, a lowest point, and a deviation from a generally planar reference surface.

17. The method according to claim 12 wherein estimating of the roll angle is estimated in accordance with the following equation:

roll=a sin [(z_RearLeft−

z_RearRight)/RearTrackWidth]

where roll is the roll angle, a sin is arc sin, z_RearLeft is the left rear wheel height, z_RearRight is right wheel height, and RearTrackWidth is the spacing between the left rear wheel and the right rear wheel of the vehicle.

18. The method according to claim 12 wherein estimating of the pitch angle is estimated in

accordance with the following equation:

pitch=a sin [(z_FrontCenter−

z_RearCenter)/WheelBase]

where pitch is the

pitch angle, a sin is arc sin, z_FrontCenter is the height of the center for the front axle, wherein z_RearCenter is the height of the center for the rear axle or wheel bearing, and wherein the Wheelbase is the spacing between the front axle and the rear axle.

19. The method according to claim 12 wherein the estimating a terrain map comprises estimating a terrain map based on range data points and color data gathered by electromagnetic perception focused in front of a vehicle.

20. The method according to claim 12 wherein the estimating at least one of further comprises estimating predicted friction coefficient data based on the terrain map for respective terrain cells and the vehicular constraint data.

21. The method according to claim 12 wherein the vehicular constraint data comprises one or more of the following:

physical specifications of the vehicle, dimensions, stability, roll-over resistance, ground clearance, turning radius, cruising speed, fuel capacity and maximum range of vehicle.

22. The method according to claim 12 wherein the adjusting of the vehicle control data comprises selecting a preferential local path plan to maintain stability of the vehicle within each terrain cell that the planned path intercepts.

23. The method according to claim 12 wherein the adjusting of the vehicle control data comprises avoiding entry into any terrain cells in which a maximum roll angle for the vehicle is predicted to be exceeded.

24. The method according to claim 12 wherein the adjusting of the vehicle control data comprises avoiding vehicle entry into any terrain cells in which a maximum pitch angle of the vehicle is predicted to be exceeded.

25. The method according to claim 12 wherein the adjusting of the vehicle control data comprises avoiding vehicle entry into any terrain cells in which a bottom of the vehicle is predicted to collide with any peaks of the ground within the terrain cell.

26. A system for estimating the navigability of a terrain comprises:

an estimator for estimating a local terrain surface map based on at least one of range data points, color data, and infrared data gathered by electromagnetic perception focused in front of a vehicle, the map being composed of a series of terrain cells;

an analyzer for estimating at least one of predicted roll data, predicted pitch data, predicted ground clearance data, and predicted friction coefficient data based on the estimated terrain map for respective terrain cells and vehicular constraint data;

a local planner for determining predicted vehicle control data for terrain cells within the terrain along a planned path of the vehicle based on the estimations;

vehicle sensors for sensing at least one of actual roll data, actual pitch data, actual ground clearance data, and actual friction coefficient data for the terrain cells when the vehicle is coextensively positioned with the corresponding terrain cell for which the analyzer performed an estimation; and

a learning module adjusts at least one of the terrain map estimation and the control data determination based on the sensed actual data.

27. The system according to claim 26 wherein the terrain map comprises a group of cells, wherein each cell is associated with an average terrain height, a lowest point, and a deviation from a generally planar reference surface.

28. The system according to claim 26 wherein the predicted roll angle is estimated in accordance with the following equation:

roll=a sin [(z_RearLeft−

z_RearRight)/RearTrackWidth]

where roll is the roll angle, a sin is arc sin, z_RearLeft is the left rear wheel height, z_RearRight is right wheel height, and RearTrackWidth is the spacing between the left rear wheel and the right rear wheel of the vehicle.

29. The system according to claim 26 wherein the predicted pitch angle is estimated in accordance with the following equation:

pitch=a sin [(z_FrontCenter−

z_RearCenter)/WheelBase]

where pitch is the pitch angle, a sin is arc sin, z_FrontCenter is the height of the center for the front axle, wherein z_RearCenter is the height of the center for the rear axle or wheel bearing, and wherein the Wheelbase is the spacing between the front axle and the rear axle.

30. The system according to claim 26 wherein the estimator estimates the terrain map based on range data points and color data gathered by electromagnetic perception focused in front of a vehicle.

31. The system according to claim 26 wherein the analyzer estimates a predicted friction coefficient data based on the estimated terrain map for respective terrain cells and the vehicular constraint data.

32. The system according to claim 26 wherein the vehicular constraint data comprises one or more of the following:

physical specifications of the vehicle, dimensions, stability, roll-over resistance, ground clearance, turning radius, cruising speed, fuel capacity and maximum range of vehicle.

33. The system according to claim 26 wherein the local planner selects a preferential local path plan to maintain stability of a vehicle within each terrain cell that the planned path intercepts.

34. The system according to claim 26 wherein the local planner selects a preferential local path plan to avoid entry into any terrain cells in which a maximum roll angle for the vehicle is predicted to be exceeded.

35. The system according to claim 26 wherein the local planner adjusts the vehicle control data to avoid vehicle entry into any terrain cells in which a maximum pitch angle of the vehicle is predicted to be exceeded.

36. The system according to claim 26 wherein the local planner adjusts the vehicle control data to avoid vehicle entry into any terrain cells in which a bottom of the vehicle would collide with any peaks of the ground within the terrain cell.