Traffic management system for a passageway environment

• US 7,756,615 B2
• Filed: 07/26/2006
• Issued: 07/13/2010
• Est. Priority Date: 07/26/2005
• Status: Active Grant

• Current Assignee: Macdonald Dettwiler and Associates Limited
• Original Assignee: Macdonald Dettwiler and Associates Limited
• Inventors: Marshall, Joshua A., Barfoot, Timothy D.
• Primary Examiner(s): Zanelli; Michael J.
• Application Number:
  US11/493,027
• Publication Number:
  US 20070027612A1
• Time in Patent Office: 1,448 Days
• Field of Search: None
• US Class Current: 701/25
• CPC Class Codes: B60W 10/04 including control of propulsion units
• CPC Class Codes: B60W 10/20 including control of steering systems
• CPC Class Codes: B60W 2050/0028 Mathematical models, e.g. for simulation
• CPC Class Codes: B60W 2552/00 Input parameters relating to infrastructure
• CPC Class Codes: B60W 2556/50 of positioning data, e.g. GPS [Global Positioning System] data
• CPC Class Codes: G05B 2219/40555 Orientation and distance
• CPC Class Codes: G05B 2219/45004 Mining
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0261 using magnetic plots
• 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/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G05D 1/0297 by controlling means in a control room
• CPC Class Codes: G08G 1/20 Monitoring the location of vehicles belonging to a group, e.g. fleet of vehicles, countable or determined number of vehicles
• CPC Class Codes: G08G 1/202 Dispatching vehicles on the basis of a location, e.g. taxi dispatching

Abstract

The present invention provides a system for coordinating multiple vehicles in a passageway environment (e.g., in underground mines). The system includes methods and apparatus for determining the global position and orientation of a vehicle in said passageway environment, and methods for planning routes and monitoring the travels of multiple vehicles in said passageway environment. A global position and orientation estimation system employs one or more odometric sensors and one or more range sensing devices. It works in three basic steps. In the first step, it records and processes sensor data that is descriptive of the passageway environment by moving the system through said passageway environment. In the second step, it generates a globally consistent map of said passageway environment. Finally, real-time localization is provided by employing odometric sensors and range sensing devices to determine the system'"'"'s global position and orientation with respect to said globally consistent map, both initially and as it travels through the passageway environment. A route planning method accepts higher-level goals for a set of multiple vehicles in said passageway environment and generates a route plan for each vehicle that minimizes the travel time for the group of vehicles, while at the same time avoiding collisions between vehicles. Route plans are sent to the vehicles for implementation and a monitoring method tracks the global positions and orientations of the vehicles and ensures that both safety and efficiency are maintained.

Claims

1. A traffic management system for one or more vehicles located in a passageway environment comprising:

a) one or more vehicles, each equipped with one or more odometric sensors, one or more range-sensing devices, and a microprocessor including memory storage, said one or more odometric sensors and one or more range-sensing devices being connected to said microprocessor, said microprocessor being programmed to acquire data from said one or more odometric sensors and said one or more range-sensing devices, and based on said data, said microprocessor being programmed for estimating global position and orientation of the vehicle on which it is located in said passageway environment; and

b) a central microprocessor that executes said route planning and vehicle monitoring, said central microprocessor being connected to said one or more vehicles by way of a wireless data communications system, and based on said estimated global position and orientation of each of said one or more vehicles, said central microprocessor being programmed for

generating strategic route plans for said one or more vehicles located in said passageway environment; and

monitoring the global position and orientation of said one or more vehicles in said passageway environment as said one or more vehicles progress along said strategic route plans.

2. The system according to claim 1 wherein said microprocessor is programmed

i) for logging raw data from sensors including scans from said one or more range-sensing devices and said one or more odometric sensors as said vehicle moves throughout said passageway environment, determining acquisition times of all raw data, and storing said raw data and acquisition times in said microprocessor memory storage

ii) pre-processing said logged data to obtain estimates of the vehicle'"'"'s positions and orientations during the logging process

iii) storing said logged and pre-processed data to a log file in said microprocessor memory storage

iv) creating a globally consistent map of said passageway environment from said logged and pre-processed data and storing said globally consistent map in said microprocessor memory storage, and

v) repeatedly determining the global position and orientation of said vehicle, in real time, as it is propelled through said passageway environment, using said one or more range-sensing devices, said one or more odometric sensors, said microprocessor, and said globally consistent map of said passageway environment stored in said microprocessor memory storage.

3. The system according to claim 2 wherein the vehicle is moved throughout said passageway environment while simultaneously logging data from sensors by an operator manually driving the vehicle through said passageway environment, or configuring said vehicle for tele-operation and remotely driving the vehicle through said passageway environment.

4. The system according to claim 2 wherein said microprocessor is programmed so that during logging of data from sensors information relating to radio frequency identification (RFID) infrastructure is acquired, if present in said passageway environment.

5. The system according to claim 4 wherein creating a globally consistent map of said passageway environment includes estimating the locations of RFID infrastructure, if present in said passageway environment, with respect to said globally consistent map.

6. The system according to claim 2 wherein said microprocessor is programmed so that during logging of data from sensors information relating to communications system infrastructure is acquired, if present in said passageway environment.

7. The system according to claim 6 wherein creating a globally consistent map of said passageway environment includes estimating the locations of communications system infrastructure, if present in said passageway environment, with respect to said globally consistent map.

8. The system according to claim 2 wherein pre-processing said logged data to obtain estimates of the system'"'"'s positions and orientations during the logging process includes processing said raw data to obtain estimates of the vehicle'"'"'s positions and orientations during logging by applying dead-reckoning based on raw data from said one or more odometric sensors and by improving said dead-reckoned estimates by comparing each range-sensing device scan to one or more scans that precede it in time.

9. The system according to claim 2 wherein said microprocessor is programmed so that creating a globally consistent map of said passageway environment includes generating a sequence of local maps of the passageway environment from said logged sensor data, said data acquisition times, and said pre-processed data stored in said microprocessor memory storage, combining said local maps of the passageway environment into one or more larger maps that are globally consistent, and storing the local maps and said globally consistent maps of said passageway environment in said microprocessor memory storage.

10. The system according to claim 9 wherein creating a globally consistent map of said passageway environment includes combining multiple globally consistent maps into a larger globally consistent map.

11. The system according to claim 9 wherein combining said local maps into one or more globally consistent maps includes using input provided by a human operator by way of a user interface.

12. The system according to claim 9 wherein said local maps are local metric maps and said one or more globally consistent maps are one or more globally consistent, and wherein combining said local metric maps into one or more globally consistent maps includes determining which local metric maps are associated and which are not, matching sub-regions of said local metric maps to one another, aligning said local metric maps with respect to one another, and subsequently optimizing the quality of said matching and alignment of all the local metric maps by minimizing a desirable error metric.

13. The system according to claim 9 wherein said local maps and said globally consistent map are metric maps, and wherein generating said local metric maps of said passageway environment includes using logged data from both said one or more odometric sensors and one or more range-sensing devices and wherein said local metric maps include grids of cells, wherein each cell takes on a cell value of either ‘

occupied’

or ‘

not occupied’

such that a cell is assigned to be ‘

occupied’

if it is estimated that the region of said passageway represented by the cell contains an obstacle, and a cell is assigned to be ‘

not occupied’

if the region of said passageway represented by the cell contains free space that is possibly traversable by a self-propelled vehicle.

14. The system according to claim 13 wherein spacing and boundaries of said local metric maps are constructed using knowledge of the sensing range of said one or more range-sensing devices, the maximum size of corridors within said passageway environment, and adrift error associated with dead-reckoning, and such that there is sufficient overlap in the regions expressed by contiguous local metric maps so as to enable combining local metric maps into a globally consistent map of said passageway environment.

15. The system according to claim 13 wherein determining the cell values for said local metric maps is done through the use of a ray-tracing algorithm to mark those cells from the system'"'"'s position out to the measured range-sensing device reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the logged data from said one or more range-sensing devices used for each local map and computing a final cell value by using the most common value for that cell.

16. The system according to claim 2 wherein repeatedly determining the global position and orientation of the vehicle, in real time, as it is moved through said passageway environment includes estimating the initial global position and orientation of the vehicle with respect to said globally consistent map before movement of the vehicle by using said one or more range-sensing devices.

17. The system according to claim 16 wherein said globally consistent map is a globally consistent metric map, and wherein estimating the initial global position and orientation of the vehicle with respect to said globally consistent metric map before movement includes selecting a finite set of feasible hypotheses representing possible initial global positions and orientations for the vehicle, locally optimizing each of said hypotheses by a suitable optimization algorithm, computing weights for each of said hypotheses by comparing data acquired from said one or more range-sensing devices to the expected value for each hypothesis given said globally consistent metric map, and determining the most likely initial global position and orientation of the vehicle from said computed weights associated with said hypotheses.

18. The system according to claim 17 wherein an error signal is reported if the weight associated with said most likely initial global position and orientation system is below some specified threshold.

19. The system according to claim 17 wherein for estimating the initial global position and orientation of the vehicle includes passing said estimate to a user interface connected to said microprocessor that requests confirmation from a user that said estimate is a reasonable estimate.

20. The system according to claim 17 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of RFID infrastructure, if present in said passageway environment.

21. The system according to claim 17 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of communications system infrastructure in said passageway environment.

22. The system according to claim 17 wherein repeatedly determining the global position and orientation of the vehicle includes determining a level of confidence associated with said global position and orientation estimate.

23. The system according to claim 16 wherein said globally consistent map is a globally consistent metric map, and wherein-repeatedly determining the global position and orientation of the vehicle includes estimating the vehicle'"'"'s position and orientation by dead-reckoning as well as using data from said one or more range-sensing devices for correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map.

24. The system according to claim 23 wherein correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map includes minimizing a suitable error between data acquired from said one or more range-sensing devices and the expected range-sensing device data for an appropriately selected set of possible positions and orientations of the vehicle given said globally consistent metric map.

25. The system according to claim 2 wherein said means to estimate each vehicle'"'"'s global position and orientation in said passageway environment is a self-contained and portable unit that can be advantageously mounted on said vehicle located in a passageway environment.

26. The system according to claim 2 wherein said estimating each vehicle'"'"'s global position and orientation in said passageway environment includes using an interface connected to said microprocessor that provides a graphical output for displaying the vehicle'"'"'s global position and orientation, in real time, with respect to said globally consistent map of said passageway environment.

27. The system according to claim 2 wherein said route planning includes generating a directed graph representation of said passageway environment, specifying a sequence of goal states for said one or more vehicles to visit based on said directed graph representation, and generating a route plan for each vehicle to sequentially visit said respective goal states.

28. The system according to claim 27 wherein said route plans for said one or more vehicles together minimize the total travel time for the group of said one or more vehicles, and wherein said route planning includes ensuring that the route plans for said one or more vehicles do not result in collisions between said vehicles if the number of vehicles is greater than one.

29. The system according to claim 28 wherein said route planning that minimizes the total travel time for the group of said one or more vehicles is based on a search of said directed graph representation of said passageway environment.

30. The system according to claim 27 wherein generating said directed graph representation of said passageway environment includes generating a sequence of waypoints that contains information relating to i) a local path in said passageway environment, and ii) a local metric map defined relative to said local path.

31. The system according to claim 30 wherein generating said directed graph representation of said passageway environment includes generating a sequence of constraints that contain information about how said local paths should be stitched together and information about how to combine said local metric maps into one or more globally consistent metric maps.

32. The system according to claim 30 wherein said local paths are constructed from sequences of local path points in said passageway environment, and wherein said local path points include information to help steer a vehicle along the path and information to help specify an appropriate speed for a vehicle travelling along the path.

33. The system according to claim 27 wherein generating said directed graph representation of said passageway environment includes generating a sequence of closely spaced waypoints so that paths traversable by the vehicles can be constructed by forming sequences of said waypoints.

34. The system according to claim 27 wherein said sequence of goal states for said one or more vehicles to visit includes desired start and end positions and orientations for each vehicle and, if appropriate, additional intermediate positions and orientations representing pause states, such as would be needed for a vehicle to acquire or deposit payload.

35. The system according to claim 27 wherein said goal states are either supplied by a user or by external supervisory control software.

36. The system according to claim 2 wherein said central microprocessor is programmed to compare, in real time, the estimated global position and orientation of each vehicle in said passageway environment to its desired position and orientation as specified by said strategic route plan, and providing notification if one or more vehicles deviate too far from their respective desired global positions and orientations as specified by said plan.

37. The system according to claim 2 wherein said central microprocessor is programmed to compare, in real time, the estimated speed of each vehicle to its desired speed as specified by said route plan and providing notification if one or more vehicles deviate too far from their respective desired speeds as specified by said route plan.

38. The system according to claim 2 wherein said central microprocessor is programmed to compare the estimated global position and orientation of each vehicle in said passageway environment with the global positions and orientations of every other vehicle in said passageway environment, determining if one or more vehicles represent a proximity danger to other vehicles, and providing immediate notification to all affected vehicles by way of said wireless data communications system if any vehicle represents a potential danger to any other vehicle.

39. The system according to claim 2 wherein said central microprocessor that executes said route planning and vehicle monitoring includes a graphical user interface to allow for user input and for human monitoring of the global positions and orientations of all vehicles with respect to a globally consistent map of said passageway environment.

40. The system according to claim 1 wherein said central microprocessor is programmed for monitoring the vehicle'"'"'s speed.

126. The method according to claim 25 wherein said step of correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map includes minimizing a suitable error between data acquired from said one or more range-sensing devices and the expected range-sensing device data for an appropriately selected set of possible positions and orientations of the vehicle given said globally consistent metric map.

41. A global position and orientation estimation system for a passageway environment comprising

one or more odometric sensors, one or more range-sensing devices, and a microprocessor including memory storage, said one or more odometric sensors and one or more range-sensing devices being connected to said microprocessor;

and

means for moving the system throughout said passageway environment, said microprocessor being programmed for

i) simultaneously logging raw data from sensors including scans from said one or more range-sensing devices and said one or more odometric sensors, determining the acquisition times of all raw data, and storing said raw data and acquisition times in said microprocessor memory storage as said system is simultaneously moved throughout said passageway environment

ii) pre-processing said logged data to obtain estimates of the system'"'"'s positions and orientations during the logging process

iii) storing said logged and pre-processed data to a log file in said microprocessor memory storage

iv) creating a globally consistent map of said passageway environment from said logged and pre-processed data and storing said globally consistent map in said microprocessor memory storage, and

v) repeatedly determining the global position and orientation of the system, in real time, as it is propelled through said passageway environment, using said one or more range-sensing devices, said one or more odometric sensors, said microprocessor, and said globally consistent map of said passageway environment stored in said microprocessor memory storage.

42. The system according to claim 41 wherein said means for moving the system throughout said passageway environment while simultaneously logging data from sensors includes mounting said system on a vehicle and an operator manually driving the vehicle through said passageway environment, or configuring said vehicle for tele-operation and remotely driving the vehicle through said passageway environment.

43. The system according to claim 41 wherein logging data from sensors includes acquiring information relating to radio frequency identification (RFID) infrastructure, if present in said passageway environment.

44. The system according to claim 43 wherein creating a globally consistent map of said passageway environment includes estimating the locations of RFID infrastructure, if present in said passageway environment, with respect to said globally consistent map.

45. The system according to claim 41 wherein logging data from sensors includes acquiring information relating to communications system infrastructure, if present in said passageway environment.

46. The system according to claim 45 wherein creating a globally consistent map of said passageway environment includes estimating the locations of communications system infrastructure, if present in said passageway environment, with respect to said globally consistent map.

47. The system according to claim 41 wherein pre-processing said logged data to obtain estimates of the system'"'"'s positions and orientations during the logging process includes processing said raw data to obtain estimates of the system'"'"'s positions and orientations during logging by applying dead-reckoning based on raw data from said one or more odometric sensors and by improving said dead-reckoned estimates by comparing each range-sensing device scan to one or more scans that precede it in time.

48. The system according to claim 41 wherein creating a globally consistent map of said passageway environment includes generating a sequence of local maps of the passageway environment from said logged sensor data, said data acquisition times, and said pre-processed data stored in said microprocessor memory storage, combining said local maps of the passageway environment into one or more larger maps that are globally consistent, and storing the local maps and said globally consistent maps of said passageway environment in said microprocessor memory storage.

49. The system according to claim 48 wherein creating a globally consistent map of said passageway environment includes combining multiple globally consistent maps into a larger globally consistent map.

50. The system according to claim 48 wherein combining said local maps into one or more globally consistent maps includes using input provided by a human operator by way of a user interface.

51. The system according to claim 48 wherein said local maps are local metric maps and said one or more globally consistent maps are one or more globally consistent metric maps, and wherein combining said local metric maps into said one or more globally consistent maps includes determining which local metric maps are associated and which are not, matching sub-regions of said local metric maps to one another, aligning said local metric maps with respect to one another, and subsequently optimizing the quality of said matching and alignment of all the local metric maps by minimizing a desirable error metric.

52. The system according to claim 48 wherein said local maps and said globally consistent map are metric maps, and wherein generating said local metric maps of said passageway environment includes using logged data from both said one or more odometric sensors and one or more range-sensing devices and wherein said local metric maps include grids of cells, wherein each cell takes on a cell value of either ‘

occupied’

or ‘

not occupied’

such that a cell is assigned to be ‘

occupied’

if it is estimated that the region of said passageway represented by the cell contains an obstacle, and a cell is assigned to be ‘

not occupied’

if the region of said passageway represented by the cell contains free space that is possibly traversable by a self-propelled vehicle.

53. The system according to claim 52 wherein spacing and boundaries of said local metric maps are constructed using knowledge of the sensing range of said one or more range-sensing devices, the maximum size of corridors within said passageway environment, and a drift error associated with dead-reckoning, and such that there is sufficient overlap in the regions expressed by contiguous local metric maps so as to enable combining local metric maps into a globally consistent map of said passageway environment.

54. The system according to claim 52 wherein determining the cell values for said local metric maps is done through the use of a ray-tracing algorithm to mark those cells from the system'"'"'s position out to the measured range-sensing device reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the logged data from said one or more range-sensing devices used for each local map and computing a final cell value by using the most common value for that cell.

55. The system according to claim 41 wherein repeatedly determining the global position and orientation of the system, in real time, as it is moved through said passageway environment includes estimating the initial global position and orientation of the system with respect to said globally consistent map before movement of the system by using said one or more range-sensing devices.

56. The system according to claim 55 wherein said globally consistent map is a globally consistent metric map, and wherein estimating the initial global position and orientation of the system with respect to said globally consistent metric map before movement includes selecting a finite set of feasible hypotheses representing possible initial global positions and orientations for the system, locally optimizing each of said hypotheses by a suitable optimization algorithm, computing weights for each of said hypotheses by comparing data acquired from said one or more range-sensing devices to the expected value for each hypothesis given said globally consistent map, and determining the most likely initial global position and orientation of the system from said computed weights associated with said hypotheses.

57. The system according to claim 56 wherein an error signal is reported if the weight associated with said most likely initial global position and orientation system is below some specified threshold.

58. The system according to claim 56 wherein estimating the initial global position and orientation of the system includes passing said estimate to a user interface connected to said microprocessor that requests confirmation from a user that said estimate is a reasonable estimate.

59. The system according to claim 56 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of RFID infrastructure in said passageway environment.

60. The system according to claim 56 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of communications system infrastructure in said passageway environment.

61. The system according to claim 56 wherein repeatedly determining the global position and orientation of the system includes determining a level of confidence associated with said global position and orientation estimate.

62. The system according to claim 55 wherein said globally consistent map is a globally consistent metric map, and wherein repeatedly determining the global position and orientation of the system includes estimating the system'"'"'s position and orientation by dead-reckoning as well using data from said one or more range-sensing devices for correcting said dead-reckoned system position and orientation using said globally consistent metric map.

63. The system according to claim 62 wherein correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map includes minimizing a suitable error between data acquired from said one or more range-sensing devices and the expected range-sensing device data for an appropriately selected set of possible positions and orientations of the system given said globally consistent metric map.

64. The system according to claim 41 wherein said global position and orientation estimation system is a self-contained and portable unit that can be advantageously mounted on a vehicle located in a passageway environment.

65. The system according to claim 41 wherein said global position and orientation estimation system includes an interface connected to said microprocessor that provides a graphical output for displaying the system'"'"'s global position and orientation, in real time, with respect to said globally consistent map of said passageway environment.

66. A traffic management method for one or more vehicles located in a passageway environment, the vehicles each being equipped with one or more odometric sensors, one or more range-sensing devices, and a microprocessor including memory storage, said one or more odometric sensors and one or more range-sensing devices being connected to said microprocessor, a central microprocessor being connected to said one or more vehicles by way of a wireless data communications system, and said method comprising the steps of:

a) generating strategic route plans for said one or more vehicles;

b) estimating global positions and orientations of said one or more vehicles in said passageway environment, wherein said step of estimating the global position and orientation of a vehicle in a passageway environment includes the steps of

i) moving the vehicle throughout said passageway environment while simultaneously logging raw data from sensors including scans from said one or more range-sensing devices and said one or more odometric sensors, determining the acquisition times of all raw data, and storing said raw data and acquisition times in said microprocessor memory storage

ii) pre-processing said logged data to obtain estimates of the vehicle'"'"'s positions and orientations during the logging process

iii) storing said logged and pre-processed data to a log file in said microprocessor memory storage

iv) creating a globally consistent map of said passageway environment from said logged and pre-processed data and storing said globally consistent map in said microprocessor memory storage, and

v) repeatedly determining the global position and orientation of the vehicle, in real time, as it is propelled through said passageway environment, using said one or more range-sensing devices, said one or more odometric sensors, said microprocessor, and said globally consistent map of said passageway environment stored in said microprocessor memory storage; and

c) monitoring the global position and orientation of said one or more vehicles in said passageway environment as said one or more vehicles progress along said strategic route plans.

67. The method according to claim 66 wherein said vehicle is moved throughout said passageway environment by an operator manually driving the vehicle through said passageway environment, or said vehicle being configured for tele-operation and remotely driving through said passageway environment.

68. The method according to claim 66 wherein data is logged from sensors that acquire information relating to radio frequency identification (RFID) infrastructure, if present in said passageway environment.

69. The method according to claim 68 wherein said globally consistent map of said passageway environment is created to include estimates of the locations of RFID infrastructure, if present in said passageway environment, with respect to said globally consistent map.

70. The method according to claim 66 wherein data is logged from sensors that acquire information relating to communications system infrastructure, if present in said passageway environment.

71. The method according to claim 70 wherein said globally consistent map of said passageway environment is created to include estimates of the locations of communications system infrastructure, if present in said passageway environment, with respect to said globally consistent map.

72. The method according to claim 66 wherein logged data is pre-processed to obtain estimates of the vehicle'"'"'s positions and orientations during logging by applying dead-reckoning based on raw data from said one or more odometric sensors and by improving said dead-reckoned estimates by comparing each range-sensing device scan to one or more scans that precede it in time.

73. The method according to claim 66 wherein said globally consistent map is constructed by generating a sequence of local maps of the passageway environment from said logged sensor data, said data acquisition times, and said pre-processed data stored in said microprocessor memory storage, combining said local maps of the passageway environment into one or more larger maps that are globally consistent, and storing the local maps and said globally consistent maps of said passageway environment in said microprocessor memory storage.

74. The method according to claim 73 wherein said globally consistent map of said passageway environment is constructed by combining multiple globally consistent maps into a larger globally consistent map.

75. The method according to claim 73 wherein said local maps are combined into one or more globally consistent maps by using input provided by a human operator by way of a user interface.

76. The method according to claim 73 wherein said local maps and said globally consistent map are metric maps, and wherein said local metric maps are combined into one or more globally consistent maps by determining which local metric maps are associated and which are not, matching sub-regions of said local metric maps to one another, aligning said local metric maps with respect to one another, subsequently optimizing the quality of said matching and alignment of all the local metric maps by minimizing a desirable error metric, and repeating said matching and alignment steps until an acceptably small error has been achieved or a specified maximum number of iterations has been reached.

77. The method according to claim 73 wherein said local maps and said globally consistent map are metric maps, and wherein said local metric maps of said passageway environment are constructed using logged data from both said one or more odometric sensors and one or more range-sensing devices and wherein said local metric maps include grids of cells, wherein each cell takes on a cell value of either ‘

occupied’

or ‘

not occupied’

such that a cell is assigned to be ‘

occupied’

if it is estimated that the region of said passageway represented by the cell contains an obstacle, and a cell is assigned to be ‘

not occupied’

if the region of said passageway represented by the cell contains free space that is possibly traversable by a self-propelled vehicle.

78. The method according to claims 77 wherein spacing and boundaries of said local metric maps are constructed using by knowledge of the sensing range of said one or more range-sensing devices, the maximum size of corridors within said passageway environment, and a drift error associated with dead-reckoning, and such that there is sufficient overlap in the regions expressed by contiguous local metric maps so as to enable said step of combining local metric maps into a globally consistent map of said passageway environment.

79. The method according to claim 77 wherein the cell values for said local metric maps are determined by using a ray-tracing algorithm to mark those cells from the vehicle'"'"'s position out to the measured range-sensing device reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the logged data from said one or more range-sensing devices used for each local map and computing a final cell value by using the most common value for that cell.

80. The method according to claim 66 wherein said step of repeatedly determining the global position and orientation of the vehicle, in real time, as it is moved through said passageway environment includes the step of estimating the initial global position and orientation of the vehicle with respect to said globally consistent map before movement of the vehicle by using said one or more range-sensing devices.

81. The method according to claim 80 wherein said globally consistent map is a metric map, and wherein said step of estimating the initial global position and orientation of the vehicle with respect to said globally consistent metric map before movement includes the steps of selecting a finite set of feasible hypotheses representing possible initial global positions and orientations for the vehicle, locally optimizing each of said hypotheses by a suitable optimization algorithm, computing weights for each of said hypotheses by comparing data acquired from said one or more range-sensing devices to the expected value for each hypothesis given said globally consistent map, and determining the most likely initial global position and orientation of the vehicle from said computed weights associated with said hypotheses.

82. The method according to claim 81 wherein an error signal is reported if the weight associated with said most likely initial global position and orientation vehicle is below some specified threshold and including the step of repeating said method for estimating the initial global position and orientation of the vehicle until said weight is above said threshold value or a specified maximum number of repetitions has been reached.

83. The method according to claim 81 wherein said step of estimating the initial global position and orientation of the vehicle includes passing said estimate to a user interface connected to said microprocessor that requests confirmation from a user that said estimate is a reasonable estimate.

84. The method according to claim 81 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of RFID infrastructure in said passageway environment.

85. The method according to claim 81 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of communications system infrastructure in said passageway environment.

86. The method according to claim 81 wherein said step of repeatedly determining the global position and orientation of the vehicle includes determining a level of confidence associated with said global position and orientation estimate.

87. The method according to claim 80 wherein said globally consistent map is a globally consistent metric map, and wherein said step of repeatedly determining the global position and orientation of the vehicle includes estimating the vehicle'"'"'s position and orientation by dead-reckoning and the step of using data from said one or more range-sensing devices for correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map.

88. The method according to claim 87 wherein said step of correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map includes minimizing a suitable error between data acquired from said one or more range-sensing devices and the expected range-sensing device data for an appropriately selected set of possible positions and orientations of the vehicle given said globally consistent metric map.

89. The method according to claim 66 wherein an interface is connected to said microprocessor that provides a graphical user interface to allow for user input and for human monitoring of the global positions and orientations of all vehicles with respect to a globally consistent map of said passageway environment.

90. The method according to claim 66 wherein said globally consistent map is a metric map, and wherein said route planning step includes the steps of generating a directed graph representation of said passageway environment, specifying a sequence of goal states for said one or more vehicles to visit based on said directed graph representation, and generating a route plan for each vehicle to sequentially visit said respective goal states.

91. The method according to claim 90 wherein said route plans for said one or more vehicles together minimize the total travel time for the group of said one or more vehicles, and wherein said route planning step includes ensuring that the route plans for said one or more vehicles do not result in collisions between said vehicles if the number of vehicles is greater than one.

92. The method according to claim 91 wherein said route planning step of minimizing the total travel time for the group of said one or more vehicles is based on a search of said directed graph representation of said passageway environment.

93. The method according to claim 90 wherein said step of generating said directed graph representation of said passageway environment includes generating a sequence of waypoints that contains information relating to i) a local path in said passageway environment, and ii) a local metric map defined relative to said local path.

94. The method according to claim 93 wherein said step of generating a directed graph representation of said passageway environment includes generating a sequence of constraints that contain information about how said local paths should be stitched together and information about how to combine said local metric maps into one or more globally consistent metric maps.

95. The method according to claim 93 wherein said local paths are constructed from sequences of local path points in said passageway environment, and wherein said local path points include information to help steer a vehicle along the path and information to help specify an appropriate speed for a vehicle travelling along the path.

96. The method according to claim 90 wherein said step of generating said directed graph representation of said passageway environment includes generating a sequence of closely spaced waypoints so that paths traversable by the vehicles can be constructed by forming sequences of said waypoints.

97. The method according to claim 90 wherein said sequence of goal states for said one or more vehicles to visit includes desired start and end positions and orientations for each vehicle and, if appropriate, additional intermediate positions and orientations representing pause states.

98. The method according to claim 90 wherein said goal states are either supplied by a user or by external supervisory control software.

99. The method according to claim 66 wherein said vehicle monitoring step includes the steps of comparing, in real time, the estimated global position and orientation of each vehicle in said passageway environment to its desired position and orientation as specified by said route planning step, and providing notification if one or more vehicles deviate too far from their respective desired global positions and orientations as specified by said route planning step.

100. The method according to claim 66 wherein said vehicle monitoring step includes the steps of comparing, in real time, the estimated speed of each vehicle to its desired speed as specified by said route planning step and providing notification if one or more vehicles deviate too far from their respective desired speeds as specified by said route planning step.

101. The method according to claim 66 wherein said vehicle monitoring step includes the steps of comparing the estimated global position and orientation of each vehicle in said passageway environment with the global positions and orientations of every other vehicle in said passageway environment, determining if one or more vehicles represent a proximity danger to other vehicles, and providing immediate notification to all affected vehicles by way of said wireless data communications system if any vehicle represents a potential proximity danger to any other vehicle.

102. The method according to claim 66 wherein said central microprocessor that executes said route planning and vehicle monitoring steps includes a graphical user interface to allow for user input and for human monitoring of the global positions and orientations of all vehicles with respect to a globally consistent map of said passageway environment.

103. The method according to claim 66 wherein step c) includes monitoring the vehicle'"'"'s speed to check whether it is exceeding the speed recommended by the route plan, or whether it is travelling too slowly.

104. A method for estimating the global position and orientation of a vehicle in a passageway environment, the vehicle being equipped with one or more odometric sensors, one or more range-sensing devices, and a microprocessor including memory storage, said one or more odometric sensors and one or more range-sensing devices being connected to said microprocessor, and said method including the steps of

a) estimating the global position and orientation of the vehicle in a passageway environment by the steps of

i) moving the vehicle throughout said passageway environment while simultaneously logging raw data from sensors including scans from said one or more range-sensing devices and said one or more odometric sensors, determining the acquisition times of all raw data, and storing said raw data and acquisition times in said microprocessor memory storage

ii) pre-processing said logged data to obtain estimates of the vehicle'"'"'s positions and orientations during the logging process

iii) storing said logged and pre-processed data to a log file in said microprocessor memory storage

iv) creating a globally consistent map of said passageway environment from said logged and pre-processed data and storing said globally consistent map in said microprocessor memory storage, and

v) repeatedly determining the global position and orientation of the vehicle, in real time, as it is propelled through said passageway environment, using said one or more range-sensing devices, said one or more odometric sensors, said microprocessor, and said globally consistent map of said passageway environment stored in said microprocessor memory storage.

105. The method according to claim 104 wherein said vehicle is moved throughout said passageway by having an operator manually drive the vehicle through said passageway environment, or said vehicle is configured for tele-operation and remotely driven through said passageway environment.

106. The method according to claim 104 wherein data is logged from sensors that acquire information relating to radio frequency identification (RFID) infrastructure, if present in said passageway environment.

107. The method according to claim 106 wherein said globally consistent map of said passageway environment is created to include estimates of the locations of RFID infrastructure, if present in said passageway environment, with respect to said globally consistent map.

108. The method according to claim 104 wherein data is logged from sensors that acquire information relating to communications system infrastructure, if present in said passageway environment.

109. The method according to claim 108 wherein said globally consistent map of said passageway environment is created to include estimates of the locations of communications system infrastructure, if present in said passageway environment, with respect to said globally consistent map.

110. The method according to claim 104 wherein logged data is pre-processed to obtain estimates of the vehicle'"'"'s positions and orientations during logging by applying dead-reckoning based on raw data from said one or more odometric sensors and by improving said dead-reckoned estimates by comparing each range-sensing device scan to one or more scans that precede it in time.

111. The method according to claim 104 wherein said globally consistent map is constructed by generating a sequence of local maps of the passageway environment from said logged sensor data, said data acquisition times, and said pre-processed data stored in said microprocessor memory storage, combining said local maps of the passageway environment into one or more larger maps that are globally consistent, and storing the local maps and said globally consistent maps of said passageway environment in said microprocessor memory storage.

112. The method according to claim 111 wherein said globally consistent map of said passageway environment is constructed by combining multiple globally consistent maps into a larger globally consistent map.

113. The method according to claim 111 wherein said local maps are combined into one or more globally consistent maps by using input provided by a human operator by way of a user interface.

114. The method according to claim 111 wherein said local maps and said globally consistent map are metric maps, and wherein said local metric maps are combined into one or more globally consistent maps by determining which local metric maps are associated and which are not, matching sub-regions of said local metric maps to one another, aligning said local metric maps with respect to one another, subsequently optimizing the quality of said matching and alignment of all the local metric maps by minimizing a desirable error metric, and repeating said matching and alignment steps until an acceptably small error has been achieved or a specified maximum number of iterations has been reached.

115. The method according to claim 111 wherein said local maps and said globally consistent map are metric maps, and wherein said local metric maps of said passageway environment are constructed using logged data from both said one or more odometric sensors and one or more range-sensing devices and wherein said local metric maps include grids of cells, wherein each cell takes on a cell value of either ‘

occupied’

or ‘

not occupied’

such that a cell is assigned to be ‘

occupied’

if it is estimated that the region of said passageway represented by the cell contains an obstacle, and a cell is assigned to be ‘

not occupied’

if the region of said passageway represented by the cell contains free space that is possibly traversable by a self-propelled vehicle.

116. The method according to claim 115 wherein spacing and boundaries of said local metric maps are constructed using by knowledge of the sensing range of said one or more range-sensing devices, the maximum size of corridors within said passageway environment, and a drift error associated with dead-reckoning, and such that there is sufficient overlap in the regions expressed by contiguous local metric maps so as to enable said step of combining local metric maps into a globally consistent map of said passageway environment.

117. The method according to claim 115 wherein the cell values for said local metric maps are determined by using a ray-tracing algorithm to mark those cells from the vehicle'"'"'s position out to the measured range-sensing device reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the logged data from said one or more range-sensing devices used for each local map and computing a final cell value by using the most common value for that cell.

118. The method according to claim 104 wherein said step of repeatedly determining the global position and orientation of the vehicle, in real time, as it is moved through said passageway environment includes the step of estimating the initial global position and orientation of the vehicle with respect to said globally consistent map before movement of the vehicle by using said one or more range-sensing devices.

119. The method according to claim 118 wherein said globally consistent map is a globally consistent metric map, and wherein said step of estimating the initial global position and orientation of the vehicle with respect to said globally consistent metric map before movement includes the steps of selecting a finite set of feasible hypotheses representing possible initial global positions and orientations for the vehicle, locally optimizing each of said hypotheses by a suitable optimization algorithm, computing weights for each of said hypotheses by comparing data acquired from said one or more range-sensing devices to the expected value for each hypothesis given said globally consistent map, and determining the most likely initial global position and orientation of the vehicle from said computed weights associated with said hypotheses.

120. The method according to claim 119 wherein an error signal is reported if the weight associated with said most likely initial global position and orientation vehicle is below some specified threshold and including the step of repeating said method for estimating the initial global position and orientation of the vehicle until said weight is above said threshold value or a specified maximum number of repetitions has been reached.

121. The method according to claim 119 wherein said step of estimating the initial global position and orientation of the vehicle includes passing said estimate to a user interface connected to said microprocessor that requests confirmation from a user that said estimate is a reasonable estimate.

122. The method according to claim 119 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of RFID infrastructure in said passageway environment.

123. The method according to claim 119 wherein said set of feasible hypotheses are selected by incorporating knowledge about the location of communications system infrastructure in said passageway environment.

124. The method according to claim 119 wherein said step of repeatedly determining the global position and orientation of the vehicle includes determining a level of confidence associated with said global position and orientation estimate.

125. The method according to claim 118 wherein said globally consistent map is a globally consistent metric map, and wherein said step of repeatedly determining the global position and orientation of the vehicle includes estimating the vehicle'"'"'s position and orientation by dead-reckoning and the step of using data from said one or more range-sensing devices for correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map.

127. The method according to claim 104 wherein an interface is connected to said microprocessor that provides a graphical output for displaying the system'"'"'s global position and orientation, in real time, with respect to said globally consistent map of said passageway environment.