Traffic management system for a passageway environment

• US 20070027612A1
• Filed: 07/26/2006
• Published: 02/01/2007
• 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, Barfoot, Timothy
• Granted Patent: US 7,756,615 B2
• Time in Patent Office: Days
• US Class Current: 701/117
• 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;

b) route planning means for generating strategic route plans for said one or more vehicles located in said passageway environment;

c) each vehicle including means to estimate its global position and orientation in said passageway environment;

d) vehicle monitoring means for 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; and

e) a central microprocessor that executes said route planning and vehicle monitoring means, said central microprocessor being connected to said one or more vehicles by way of a wireless data communications system.

2. The system according to claim 1 wherein said means for each vehicle to estimate its global position and orientation in said passageway environment includes

i) means for logging raw data from sensors including 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

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

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

iv) means for 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, and

v) means for 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.

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

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

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

6. The system according to claim 2 wherein said means for 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 rangefinder scan to one or more scans that precede it in time.

7. The system according to claim 2 wherein said means for creating a globally consistent map of said passageway environment includes means for 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, 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.

8. The system according to claim 7 wherein said means for creating a globally consistent map of said passageway environment includes combining multiple globally consistent maps into a larger globally consistent map.

9. The system according to claim 7 wherein said means for 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, such as a graphical user interface connected to said microprocessor.

10. The system according to claim 7 wherein said local maps and said globally consistent map are metric maps, and wherein said means for 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.

11. The system according to claim 7 wherein said local maps and said globally consistent map are metric maps, and wherein said means for 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, such as a wall or other object, 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.

12. The system according to claim 11 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 said means for combining local metric maps into a globally consistent map of said passageway environment.

13. The system according to claim 11 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 rangefinder reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the laser data used for each local map and computing a final cell value by using the most common value for that cell.

14. The system according to claim 4 wherein said means for 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.

15. The system according to claim 5 wherein said means for 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.

16. The system according to claim 2 wherein said means for repeatedly determining the global position and orientation of the vehicle, in real time, as it is moved through said passageway environment includes a means for 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 metric map, and wherein said means for 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 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 said means 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 16 wherein said globally consistent map is a metric map, and wherein said means for repeatedly determining the global position and orientation of the vehicle includes estimating the vehicle'"'"'s position and orientation by dead-reckoning as well as means for using data from said one or more rangefinder sensors for correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map.

23. The system according to claim 22 wherein said means for 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 rangefinder sensors and the expected rangefinder data for an appropriately selected set of possible positions and orientations of the vehicle given said globally consistent metric map.

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

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 means to estimate each vehicle'"'"'s global position and orientation in said passageway environment includes 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 means includes means for 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 means 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 27 wherein said means for generating said directed graph representation of said passageway environment includes generating a sequence of waypoints that contain information such as

i) a local path in said passageway environment, and

ii) a local metric map defined relative to said local path.

30. The system according to claim 29 wherein said means for 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.

31. The system according to claim 29 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.

32. The system according to claim 27 wherein said means for 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.

33. 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.

34. The system according to claim 27 wherein said goal states are either supplied by a user or by external supervisory control software, such as a multiple vehicle dispatching system.

35. The system according to claim 28 wherein said route planning means 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.

36. The system according to claim 2 wherein said vehicle monitoring means includes 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 means, 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 means.

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

38. The system according to claim 2 wherein said vehicle monitoring means includes 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 danger to other vehicles, for example by becoming too close to each other, 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 means 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. A global position and orientation estimation system for a passageway environment comprising

a) 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

b) means for estimating the global position and orientation of the system in a passageway environment comprising

i) means for moving the system throughout said passageway environment while simultaneously logging raw data from sensors including 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

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

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

iv) means for 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, and

v) means for 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.

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

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

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

44. The system according to claim 40 wherein said means for 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 rangefinder scan to one or more scans that precede it in time.

45. The system according to claim 40 wherein said means for creating a globally consistent map of said passageway environment includes means for 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, 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.

46. The system according to claim 45 wherein said means for creating a globally consistent map of said passageway environment includes combining multiple globally consistent maps into a larger globally consistent map.

47. The system according to claim 45 wherein said means for combining said local metric maps into one or more globally consistent maps includes using input provided by a human operator by way of a user interface, such as a graphical user interface connected to said microprocessor.

48. The system according to claim 45 wherein said local maps and said globally consistent map are metric maps, and wherein said means for 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.

49. The system according to claim 45 wherein said local maps and said globally consistent map are metric maps, and wherein said means for 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, such as a wall or other object, 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.

50. The system according to claim 49 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 said means for combining local metric maps into a globally consistent map of said passageway environment.

51. The system according to claim 49 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 rangefinder reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the laser data used for each local map and computing a final cell value by using the most common value for that cell.

52. The system according to claim 42 wherein said means for 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.

53. The system according to claim 43 wherein said means for 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.

54. The system according to claim 40 wherein said means for repeatedly determining the global position and orientation of the system, in real time, as it is moved through said passageway environment includes a means for 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.

55. The system according to claim 54 wherein said globally consistent map is a metric map, and wherein said means for 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.

56. The system according to claim 55 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.

57. The system according to claim 55 wherein said means for 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.

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

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

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

61. The system according to claim 60 wherein said means for 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 rangefinder sensors and the expected rangefinder data for an appropriately selected set of possible positions and orientations of the system given said globally consistent metric map.

62. The system according to claim 55 wherein said means for repeatedly determining the global position and orientation of the system includes determining a level of confidence associated with said global position and orientation estimate.

63. The system according to claim 40 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.

64. The system according to claim 40 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.

65. 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; 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.

66. The method according to claim 65 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 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

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, 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.

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 66 wherein data is logged from sensors that acquire information relating to communications system infrastructure, if present in said passageway environment.

70. 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 rangefinder scan to one or more scans that precede it in time.

71. 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, 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.

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

73. The method according to claim 71 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, such as a graphical user interface connected to said microprocessor.

74. The method according to claim 71 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.

75. The method according to claim 71 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, such as a wall or other object, 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.

76. The method according to claim 75 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.

77. The method according to claim 75 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 rangefinder reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the laser data used for each local map and computing a final cell value by using the most common value for that cell.

78. 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.

79. The method according to claim 69 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.

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 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 80 wherein said globally consistent map is a 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 rangefinder sensors for correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map.

87. The method according to claim 86 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 rangefinder sensors and the expected rangefinder data for an appropriately selected set of possible positions and orientations of the vehicle given said globally consistent metric map.

88. 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.

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 90 wherein said step of generating said directed graph representation of said passageway environment includes generating a sequence of waypoints that contain information such as

i) a local path in said passageway environment, and

ii) a local metric map defined relative to said local path.

93. The method according to claim 92 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.

94. The method according to claim 92 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.

95. 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.

96. 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, such as would be needed for a vehicle to acquire or deposit payload.

97. The method according to claim 90 wherein said goal states are either supplied by a user or by external supervisory control software, such as a multiple vehicle dispatching system.

98. 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.

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 danger to other vehicles, for example by becoming too close to each other, 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.

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. 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 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

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, 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.

104. The method according to claim 103 wherein said vehicle is moved throughout 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.

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

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

107. The method according to claim 103 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 rangefinder scan to one or more scans that precede it in time.

108. The method according to claim 103 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, 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.

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

110. The method according to claim 108 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, such as a graphical user interface connected to said microprocessor.

111. The method according to claim 108 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.

112. The method according to claim 108 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, such as a wall or other object, 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.

113. The method according to claim 112 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.

114. The method according to claim 112 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 rangefinder reading as ‘

not occupied’ and

cells a short distance beyond as ‘

occupied’

, then taking a tally of the cell values over all the laser data used for each local map and computing a final cell value by using the most common value for that cell.

115. The method according to claim 105 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.

116. The method according to claim 106 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.

117. The method according to claim 103 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.

118. The method according to claim 117 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 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.

119. The method according to claim 118 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.

120. The method according to claim 118 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.

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

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

123. The method according to claim 117 wherein said globally consistent map is a 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 rangefinder sensors for correcting said dead-reckoned vehicle position and orientation using said globally consistent metric map.

124. The method according to claim 123 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 rangefinder sensors and the expected rangefinder data for an appropriately selected set of possible positions and orientations of the vehicle given said globally consistent metric map.

125. The method according to claim 118 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.

126. The method according to claim 103 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.