US20120303255A1

Movatterモバイル変換

Info

Publication number: US20120303255A1
Application number: US13/116,600
Authority: US
Inventors: Lisa Wong; Andrew Evan Graham; Christopher W. Goode
Original assignee: INRO Tech Ltd
Current assignee: IMI Lakeside NZ Ltd
Priority date: 2011-05-26
Filing date: 2011-05-26
Publication date: 2012-11-29

Abstract

A method and apparatus for providing accurate localization at an industrial vehicle is described. In one embodiment, the method includes processing at least one sensor input message from a plurality of sensor devices, wherein the at least one sensor input message comprises information regarding environmental features, determining pose measurements associated with the industrial vehicle in response to each acquisition time of the at least one sensor input message and updating map data with the determined pose measurements.

Description

BACKGROUND

1. Technical Field

Embodiments of the present invention generally relate to industrial vehicle automation and, more particularly, to a method and apparatus for providing accurate localization for an industrial vehicle.

2. Description of the Related Art

Entities regularly operate numerous facilities in order to meet supply and/or demand goals. For example, small to large corporations, government organizations and/or the like employ a variety of logistics management and inventory management paradigms to move objects (e.g., raw materials, goods, machines and/or the like) into a variety of physical environments (e.g., warehouses, cold rooms, factories, plants, stores and/or the like). A multinational company may build warehouses in one country to store raw materials for manufacture into goods, which are housed in a warehouse in another country for distribution into local retail markets. The warehouses must be well-organized in order to maintain and/or improve production and sales. If raw materials are not transported to the factory at an optimal rate, fewer goods are manufactured. As a result, revenue is not generated for the unmanufactured goods to counterbalance the costs of the raw materials.

Unfortunately, physical environments, such as warehouses, have several limitations that prevent timely completion of various tasks. Warehouses and other shared use spaces, for instance, must be safe for a human work force. Some employees operate heavy machinery and industrial vehicles, such as forklifts, which have the potential to cause severe or deadly injury. Nonetheless, human beings are required to use the industrial vehicles to complete tasks, which include object handling tasks, such as moving pallets of goods to different locations within a warehouse. Most warehouses employ a large number of forklift drivers and forklifts to move objects. In order to increase productivity, these warehouses simply add more forklifts and forklift drivers.

In order to mitigate the aforementioned problems, some warehouses utilize equipment for automating these tasks. As an example, these warehouses may employ automated industrial vehicles, such as forklifts, to carry objects on paths. When automating an industrial vehicle a key requirement is the ability to accurately locate the vehicle in the warehouse; to achieve this, a plurality of sensors are frequently used to measure the position. However, it is necessary to account for distortion (i.e., time and/or motion distortion) errors caused by disparate sensor devices. Some of these distortion errors are caused by internal system delays. Other causes of distortion errors include different vehicle pose information publishing rates. Sensor data must be accurate when used for time critical tasks, such as driving.

Therefore, there is a need in the art for a method and apparatus for providing accurate localization by correcting for time distortion during industrial vehicle operation.

SUMMARY

Various embodiments of the present disclosure comprise a method and apparatus for providing accurate localization at an industrial vehicle. In one embodiment, the method includes processing at least one sensor input message from a plurality of sensor devices, wherein the at least one sensor input message comprises information regarding environmental features, determining pose measurements associated with the industrial vehicle in response to each acquisition time of the at least one sensor input message and vehicle state with the determined pose measurements.

BRIEF DESCRIPTION OF THE DRAWINGS

So that the manner in which the above recited features of the present invention can be understood in detail, a more particular description of the invention, briefly summarized above, may be had by reference to embodiments, some of which are illustrated in the appended drawings. It is to be noted, however, that the appended drawings illustrate only typical embodiments of this invention and are therefore not to be considered limiting of its scope, for the invention may admit to other equally effective embodiments.

FIG. 1 is a perspective view of a physical environment comprising various embodiments of the present disclosure;

FIG. 2 illustrates a perspective view of the forklift for navigating a physical environment to perform various tasks according to one or more embodiments;

FIG. 3 is a structural block diagram of a system for providing accurate localization for an industrial vehicle according to one or more embodiments;

FIG. 4 is a functional block diagram of a system for providing accurate localization for an industrial vehicle according to one or more embodiments;

FIG. 5 illustrates motion and time distortion associated with vehicle movement within the physical environment according to one or more embodiments;

FIG. 6 illustrates a planar laser scanner performing a laser scan within a field of view according to one or more embodiments;

FIGS. 7A-B are interaction diagrams illustrating a localization process for an industrial vehicle according to one or more embodiments;

FIG. 8 is an exemplary timing diagram illustrating sensor input message processing according to one or more embodiments;

FIG. 9 illustrates a portion of the sensor input message processing according to one or more embodiments;

FIG. 10 is a functional block diagram illustrating a localization and mapping system for navigating an industrial vehicle according to one or more embodiments;

FIG. 11 is a flow diagram of a method for providing accurate localization for an industrial vehicle according to one or more embodiments; and

FIG. 12 is a flow diagram of a method for updating a vehicle state for an industrial vehicle using a filter according to one or more embodiments.

DETAILED DESCRIPTION

FIG. 1A illustrates a schematic, perspective view of aphysical environment100 comprising one or more embodiments of the present disclosure.

In some embodiments, thephysical environment100 includes avehicle102 that is coupled to amobile computer104, acentral computer106 as well as asensor array108. Thesensor array108 includes a plurality of devices for analyzing various objects within thephysical environment100 and transmitting data (e.g., image data, video data, range map data, three-dimensional graph data and/or the like) to themobile computer104 and/or thecentral computer106, as explained further below. Thesensor array108 includes various types of sensors, such as encoders, ultrasonic range finders, laser range finders, pressure transducers and/or the like.

Thephysical environment100 further includes afloor110 supporting a plurality of objects. The plurality of objects include a plurality ofpallets112, a plurality ofunits114 and/or the like as explained further below. Thephysical environment100 also includes various obstructions (not pictured) to the proper operation of thevehicle102. Some of the plurality of objects may constitute as obstructions along various paths (e.g., pre-programmed or dynamically computed routes) if such objects disrupt task completion.

Thephysical environment100 also includes a plurality ofmarkers116. The plurality ofmarkers116 are illustrated as objects attached to a ceiling and thefloor110, but may be located throughout thephysical environment100. In some embodiments, the plurality ofmarkers116 are beacons that facilitate environment based navigation as explained further below. The plurality ofmarkers116 as well as other objects around thephysical environment100 form environment features. Themobile computer104 extracts the environment features and determines an accurate, current vehicle pose.

Thephysical environment100 may include a warehouse or cold store for housing the plurality ofunits114 in preparation for future transportation. Warehouses may include loading docks to load and unload the plurality of units from commercial vehicles, railways, airports and/or seaports. The plurality ofunits114 generally includes various goods, products and/or raw materials and/or the like. For example, the plurality ofunits114 may be consumer goods that are placed on ISO standard pallets and loaded into pallet racks by forklifts to be distributed to retail stores. Thevehicle102 facilitates such a distribution by moving the consumer goods to designated locations where commercial vehicles (e.g., trucks) load and subsequently deliver the consumer goods to one or more target destinations.

According to one or more embodiments, thevehicle102 may be an automated guided vehicle (AGV), such as an automated forklift, which is configured to handle and/or move the plurality ofunits114 about thefloor110. Thevehicle102 utilizes one or more lifting elements, such as forks, to lift one ormore units114 and then, transport theseunits114 along a path to be placed at a designated location. Alternatively, the one ormore units114 may be arranged on apallet112 of which thevehicle102 lifts and moves to the designated location.

Each of the plurality ofpallets112 is a flat transport structure that supports goods in a stable fashion while being lifted by thevehicle102 and/or another jacking device (e.g., a pallet jack and/or a front loader). Thepallet112 is the structural foundation of an object load and permits handling and storage efficiencies. Various ones of the plurality ofpallets112 may be utilized within a rack system (not pictured). Within a typical rack system, gravity rollers or tracks allow one ormore units114 on one ormore pallets112 to flow to the front. The one ormore pallets112 move forward until slowed or stopped by a retarding device, a physical stop or anotherpallet112.

In some embodiments, themobile computer104 and thecentral computer106 are computing devices that control thevehicle102 and perform various tasks within thephysical environment100. Themobile computer104 is adapted to couple with thevehicle102 as illustrated. Themobile computer104 may also receive and aggregate data (e.g., laser scanner data, image data and/or any other related sensor data) that is transmitted by thesensor array108. Various software modules within themobile computer104 control operation of hardware components associated with thevehicle102 as explained further below.

In some embodiments,FIG. 1 illustrates an industrial area having forklifts equipped with various sensor devices, such as a laser scanner, an encoder and a camera. As explained further below, themobile computer104 calculates a vehicle pose (e.g., position and orientation) using a series of measurements, such as wheel rotations. One or more sensor devices are coupled to the wheels and provide an independent measurement of distance travelled by each of these wheels from which odometry data is calculated. Alternatively an IMU may be used to measure odometry data. One or more two-dimensional laser scanners provide details of thephysical environment100 in the form of range readings and their corresponding angles from thevehicle102. From the laser data, themobile computer104 extracts features associated with landmarks, such as straight lines, corners, arcs, markers and/or the like. A camera may provide three-dimensional information including height measurements. Landmarks may also be extracted from the camera data based on various characteristics, such as color, size, depth, position, orientation, texture and/or the like, in addition to the extracted features.

Using a filter (e.g., an Extended Kalman Filter (EKF)), themobile computer104 models the pose of the vehicle in the two-dimensional plane (i.e. the (x, y) coordinates and the heading of the vehicle102) as a probability density. The odometry data is used for predicting the updated pose of the vehicle, and the environmental markers extracted from the laser scan can be compared with a known map and/or a list of dynamic landmarks maintained by the filter to correct for error in the vehicle pose.

FIG. 2 illustrates a perspective view of theforklift200 for performing various tasks within a physical environment according to one or more embodiments of the present disclosure.

The forklift200 (i.e., a lift truck, a high/low, a stacker-truck, trailer loader, side-loader or a fork hoist) is a powered industrial truck having various load capacities and used to lift and transport various objects. In some embodiments, theforklift200 is configured to move one or more pallets (e.g., thepallets112 ofFIG. 1) of units (e.g., theunits114 ofFIG. 1) along paths within the physical environment (e.g., thephysical environment100 ofFIG. 1). The paths may be pre-defined or dynamically computed as tasks are received. Theforklift200 may travel inside a storage bay that is multiple pallet positions deep to place or retrieve a pallet. Oftentimes, theforklift200 places the pallet on cantilevered arms or rails. Hence, the dimensions of theforklift200, including overall width and mast width, must be accurate when determining an orientation associated with an object and/or a target destination.

Theforklift200 typically includes two or more forks (i.e., skids or tines) for lifting and carrying units within the physical environment. Alternatively, instead of the two or more forks, theforklift200 may include one or more metal poles (not pictured) in order to lift certain units (e.g., carpet rolls, metal coils and/or the like). In one embodiment, theforklift200 includes hydraulics-powered, telescopic forks that permit two or more pallets to be placed behind each other without an aisle between these pallets.

Theforklift200 may further include various mechanical, hydraulic and/or electrically operated actuators according to one or more embodiments. In some embodiments, theforklift200 includes one or more hydraulic actuators (not labeled) that permit lateral and/or rotational movement of two or more forks. In one embodiment, theforklift200 includes a hydraulic actuator (not labeled) for moving the forks together and apart. In another embodiment, theforklift200 includes a mechanical or hydraulic component for squeezing a unit (e.g., barrels, kegs, paper rolls and/or the like) to be transported.

Theforklift200 may be coupled with themobile computer104, which includes software modules for operating theforklift200 in accordance with one or more tasks. Theforklift200 is also coupled with an array comprising various sensor devices (e.g., thesensor array108 ofFIG. 1), which transmits sensor data (e.g., image data, video data, range map data and/or three-dimensional graph data) to themobile computer104 for extracting information associated with environmental features. These devices may be mounted to theforklift200 at any exterior and/or interior position or mounted at known locations around thephysical environment100. Exemplary embodiments of theforklift200 typically includes acamera202, aplanar laser scanner204 attached to each side and/or anencoder206 attached to eachwheel208. In other embodiments, theforklift200 includes only theplanar laser scanner204 and theencoder206. Theforklift200 may use any sensor array with a field of view that extends to a current direction of motion (e.g., travel forwards, backwards, fork motion up/down, reach out/in and/or the like). These encoders determine motion data related to vehicle movement. Externally mounted sensors may include laser scanners or cameras positioned where the rich data set available from such sensors would enhance automated operations. External sensors may include a limited set transponders and/or other active or passive means by which an automated vehicle could obtain an approximate position and/or process within a filter for determining vehicle state.

In some embodiments, a number of the sensor devices (e.g., laser scanners, laser range finders, encoders (i.e., odometry), pressure transducers and/or the like) as well as their position on theforklift200 are vehicle dependent and the position at which these sensors are mounted affects the processing of the measurement data. For example, by ensuring that all of the laser scanners are placed at a measureable position, the sensor array may process the laser scan data and transpose it to a center point for theforklift200. Furthermore, the sensor array may combine multiple laser scans into a single virtual laser scan, which may be used by various software modules to control theforklift200.

FIG. 3 is a structural block diagram of asystem300 for providing accurate localization for an industrial vehicle according to one or more embodiments. In some embodiments, thesystem300 includes themobile computer104, thecentral computer106 and thesensor array108 in which each component is coupled to each other through anetwork302.

Themobile computer104 is a type of computing device (e.g., a laptop, a desktop, a Personal Desk Assistant (PDA) and the like) that comprises a central processing unit (CPU)304, various support circuits306 and amemory308. TheCPU304 may comprise one or more commercially available microprocessors or microcontrollers that facilitate data processing and storage. Various support circuits306 facilitate operation of theCPU304 and may include clock circuits, buses, power supplies, input/output circuits and/or the like. Thememory308 includes a read only memory, random access memory, disk drive storage, optical storage, removable storage, and the like. Thememory308 includes various data, such as apriority queue310 havingsensor input messages312 and timestamps312, posemeasurement data316 andvehicle state information318. Eachtimestamp314 indicates an acquisition time for a corresponding one of thesensor input messages312. Thememory308 includes various software packages, such as an environment basednavigation module320.

Thecentral computer106 is a type of computing device (e.g., a laptop computer, a desktop computer, a Personal Desk Assistant (PDA) and the like) that comprises a central processing unit (CPU)322,various support circuits324 and amemory326. TheCPU322 may comprise one or more commercially available microprocessors or microcontrollers that facilitate data processing and storage.Various support circuits324 facilitate operation of theCPU322 and may include clock circuits, buses, power supplies, input/output circuits and/or the like. Thememory326 includes a read only memory, random access memory, disk drive storage, optical storage, removable storage, and the like. Thememory326 includes various software packages, such as amanager328, as well as various data, such astasks330.

Thenetwork302 comprises a communication system that connects computers by wire, cable, fiber optic, and/or wireless links facilitated by various types of well-known network elements, such as hubs, switches, routers, and the like. Thenetwork302 may employ various well-known protocols to communicate information amongst the network resources. For example, thenetwork302 may be part of the Internet or intranet using various communications infrastructure such as Ethernet, WiFi, WiMax, General Packet Radio Service (GPRS), and the like.

Thesensor array108 is communicable coupled to themobile computer104, which is attached to an automated vehicle, such as a forklift (e.g., theforklift200 ofFIG. 2). Thesensor array108 includes a plurality ofdevices332 for monitoring a physical environment and capturing various observations, which is stored by themobile computer104 as thesensor input messages312. In some embodiments, thesensor array108 may include any combination of devices, such as one or more laser scanners, encoders, cameras and/or the like. For example, a laser scanner may be attached to a lift carriage at a position above or below the forks. Alternatively, the laser scanner may be a planar laser scanner that is located in a fixed position on the forklift body where its field of view extends to cover the direction of travel of the forklift. The plurality ofdevices332 may also be distributed throughout the physical environment at fixed and/or moving positions.

In some embodiments, thepose measurement data316 includes an aggregation of the sensor data that is transmitted by and represent observations of the plurality ofdevices332 regarding the physical environment. The aggregated sensor data may include information associated with static and/or dynamic environmental features. In some embodiments, thepose measurement data316 is corrected with respect to time and/or motion distortion in order to determine a current vehicle pose and update thevehicle state information318 as explained further below.

Thepriority queue310 stores observed sensor data over a period of time in the form of thesensor input messages312 along with data sources and themeasurement time stamps314. In some embodiments, the environment basednavigation module320 inserts eachsensor input message312 into thepriority queue310 based on a priority. The environment basednavigation module320 uses various factors, such as an acquisition time, to determine the priority for each of thesensor input message312 according to some embodiments.

Thevehicle state information318 describes one or more states (e.g., a previous and/or a current vehicle state) of the vehicle at various times k_i. In some embodiments, thevehicle state information318 includes an estimate of vehicle position and/or orientation of which the present disclosure may refer to as the pose prediction. In some embodiments, thevehicle state information318 includes an update of the pose prediction in view of a previous vehicle pose, odometry data and/or laser scanner data. In some embodiments, thevehicle state information318 includes vehicle velocity and other motion data related to vehicle movement. For example, the other motion data is a temporal characteristic representing distortion caused by the vehicle movement during a laser scan.

The environment basednavigation module320 uses a filter (e.g., a process filter, such as an extended Kalman Filter) to produce a pose prediction based on a prior vehicle state and then, update the pose prediction using thepose measurement data310. Based on odometry data from thesensor array108, such as an encoder attached to a wheel, or other pose prediction data such as data from an Intertial Measurement Unit, the environment basednavigation module320 estimates a current vehicle state. Using a wheel diameter, for example, the environment basednavigation module320 computes the distance traveled by theindustrial vehicle102 from a prior vehicle position. As another example, the encoder may directly measure surface velocity of the wheel and communicate such a measurement to the environment basednavigation module320. This information about distance travelled is integrated with the previously calculated vehicle state estimate to give a new vehicle state estimate. The environment basednavigation module320 may also use the filter to estimate uncertainty and/or noise associated with the current vehicle state (e.g., vehicle pose).

The environment basednavigation module320 accesses thepriority queue310 and examines thesensor input messages312 in order of reception time. In some embodiments, the environment basednavigation module320 rearranges (e.g., sorts) thesensor input messages312 prior to updating thevehicle state information318. Thesensor input messages312 are to be rearranged according to internal system delays and/or characteristic measurement delays associated with a sensor. Each data source has a measureable internal system delay, which can be used as an estimate of the measurement time. Processing the rearrangedsensor input messages312 enables accurate localization and mapping because the order at which thesensor input messages312 are retrieved is a same order at which the data in thesensor input messages312 is acquired by thesensor devices332.

In some embodiments, the environment basednavigation module320 performs an observation-update step in the order of the acquisition time instead of reception time. Based on the prior vehicle state and the current pose prediction, the environment basednavigation module320 executes a data fusion technique to integrate available odometry data and correct the current pose prediction. The environment basednavigation module320 uses the current pose prediction to update thevehicle state information318 with an accurate vehicle position and/or heading.

FIG. 4 is a functional block diagram of asystem400 for providing accurate localization for an industrial vehicle according to one or more embodiments. Thesystem400 includes themobile computer104, which couples to an industrial vehicle, such as a forklift, as well as thesensor array108. Various software modules within themobile computer104 collectively form an environment based navigation module (e.g., the environment basednavigation module320 ofFIG. 3).

Themobile computer104 includes various software modules (i.e., components) for performing navigational functions, such as alocalization module402, amapping module404, acorrection module408, and avehicle controller410. Themobile computer104 provides accurate localization for the industrial vehicle and updates mapdata406 with information associated with environmental features. Thelocalization module402 also includes various components, such as afilter414 and afeature extraction module416, for determining avehicle state418. Themap module404 includes various data, such asdynamic features422 and static features. Themap module404 also includes various components, such as afeature selection module420.

In some embodiments, thecorrection module408 processes one or more sensor input data messages and examines observed sensor data therein. Thecorrection module408 eliminates motion and/or time distortion artifacts prior to being processed by thefilter414.

FIG. 5 illustrates theplanar laser scanner204 performing alaser scan500 within a field of view according to one or more embodiments. As mentioned above, theforklift200 may be moving in a particular direction (e.g., forward) during thelaser scan500. As described in detail further below, a mobile computer (e.g., themobile computer104 ofFIG. 1) executes an environment based navigation module, which corrects laser scanner data to account for vehicle movement resulting in accurate localization.

Between a field of view from P₁to P₂, theplanar laser scanner204 performs the laser scan during scan time (T_s)502. A time period required for processing the laser scanner data is stored as processing time (T_p). Next, the laser scanner data is transmitted to a process filter in a form of a sensor input message during a transmission time (T_t)506. Collectively, theT_s502, theT_p504 and theT_t506 constitute a latency between acquisition and availability of the laser scanner data to the process filter for updating a vehicle state. The environment based navigation module accounts for such a latency using a constant value (e.g., a sum of values consisting of one-half of theT_s502, theT_p504 and the T_t506). If theT_p504 cannot be computed because the internal processing of the laser is unknown, the process filter uses a time associated with the availability of the sensor input message and a publication rate (i.e., periodicity of laser scanning) to estimate theT_p504.

FIG. 6 illustrates motion distortion associated with thevehicle102 movement within thephysical environment100 according to one or more embodiments. Specifically, thevehicle102 is depicted as moving closer to awall604 during a laser scan by various sensor devices, such as planar laser scanners. As these sensor devices are capturing laser scanner data, thevehicle102 starts atpose600, moves in a straight forward direction and finally, ends at apose602. Thevehicle102 movement causes motion artifacts in the laser scanner data that distort coordinates of various environmental features. The motion of the vehicle during the scan causes anestimation error608 in the angle of the wall resulting in the wall position being estimated as that shown as606. Those skilled in the art will realize that rotational motion of the laser scanner may cause more complex distortions of observed features which, if uncorrected, will create significant errors in the vehicle position estimate. These errors grow as the velocity of the vehicle increases.

The environment based navigation module collects vehicle motion data, such as odometry data, during theT_s502 and corrects the planar laser scanner data. In some embodiments, the vehicle motion data includes parameters for determining a distance and/or direction traveled, which is used to adjust coordinates associated various environmental features. After removing motion artifacts caused by the vehicle movement, the environment based navigation module uses theT_s502 to update a previous vehicle pose prediction and determine a current vehicle state.

In some embodiments, thecorrection module408 inserts the one or more sensor input data messages into a queue. Thecorrection module408 subsequently sorts the sensor input messages based on the corrected acquisition time. When a sensor input message from a trigger data source become available to thecorrection module408, a filter update process is performed on the queue by thelocalization module402, which integrates remaining sensor data into the pose measurements to determine a current vehicle pose

In addition to thefilter414 for calculating thevehicle state418, thelocalization module402 also includes thefeature extraction module416 for extracting known standard features from the corrected sensor data. Themap module404 compares thevehicle state418 with thedynamic features422 and/or thestatic features422 in order to eliminate unrelated features, which reduce a total number of features to examine. Thefeature selection module420 manages addition and modification of thedynamic features420 to themap data406. Thefeature selection module420 can update themap data406 to indicate areas recently occupied or cleared of certain features, such as known placed and picked items.

After comparing these pose measurements with a pose prediction, thefilter414 corrects the pose prediction to account for an incorrect estimation and/or observation uncertainty and updates thevehicle state418. Thefilter414 determines thevehicle state418 and instructs themapping module404 to update themap data406 with information associated with the dynamic features422. Thevehicle state418, which is modeled by thefilter414, refers to a current vehicle state and includes data that indicate vehicle position (e.g., coordinates) and/or orientation (e.g., degrees) as well as movement (e.g., vehicle velocity, acceleration and/or the like). Thelocalization module408 communicates data associated with thevehicle state418 to themapping module404 while also communicating such data to thevehicle controller410. Based on the vehicle position and orientation, thevehicle controller410 navigates the industrial vehicle to a destination.

It is appreciated that thesystem400 may employ several computing devices to perform environment based navigation. Any of the software modules within thecomputing device104 may be deployed on different or multiple physical hardware components, such as other computing devices. Themapping module404, for instance, may be executed on a server computer (e.g., thecentral computer102 ofFIG. 1) over a network (e.g., thenetwork302 ofFIG. 3) to connect with multiple mobile computing devices for the purpose of sharing and updating themap data406 with a current vehicle position and orientation.

In some embodiments, thecorrection module408 processes sensor input messages from disparate data sources, such as thesensor array108, having different sample/publish rates for thevehicle state418 as well as different (internal) system delays. Due to the different sampling periods and system delays, the order at which the sensor input messages are acquired is not the same as the order at which the sensor input messages eventually became available to thecomputing device104. Thefeature extraction module416 extracts observed pose measurements from the sensor data within these messages. Thelocalization module402 examines each message separately in order to preserve the consistency of each observation. Such an examination may be performed instead of fusing the sensor data to avoid any dead reckoning errors.

FIG. 7A is an interaction diagram illustrating a localization andmapping process700 for an industrial vehicle according to one or more embodiments. Specifically, the localization andmapping process700 includes processing and communicating various data between components or layers, such assensor data correction702, aninterface704, feature extraction707,data association708,EKF710 anddynamic map712. The localization andmapping process700 supports industrial vehicle operation using primarily environmental features. Theinterface704 facilitates control over the layers and is added to an environment based navigation module.

Thefeature extraction706 examines data inputted by sensor devices and extracts observed features (e.g. lines and corners). Thedata association708 compares the observed features with known feature information to identify matching features with existing static424 and/or dynamic422 map data. TheEKF710 is an Extended Kalman Filter that, given measurements associated with the matching features and a previous vehicle pose, provides a most likely current vehicle pose. Thedynamic map manager712 maintains an up-to-date dynamic map of features used for localization that are not found in a-priori static map.

FIG. 7B is an interaction diagram illustrating alocalization process714 using motion data associated with the industrial vehicle according to one or more embodiments. The vehicle motion data refers to industrial vehicle movement, which may distort pose predictions determined by theEKF710. For example, the industrial vehicle may be moving as sensor input messages are acquired from the sensor devices (e.g., during a laser scan). These sensor input messages include imprecise sensor data that eventually result in the distorted pose predictions and an inaccurate estimate of a next vehicle state.

Thesensor data correction702 is a step in thelocalization process714 where motion artifacts are removed from the sensor data prior to a vehicle pose prediction according to some embodiments. Thesensor data correction702 processes the vehicle motion data, which is determined from various sensor data and then, communicated to theinterface704. For example, thesensor data correction702 uses a wheel diameter and odometry data to compute velocity measurements. A change in vehicle pose causes the motion artifacts in subsequent laser scanner data. Accordingly, thesensor data correction702 modifies the laser scanner data prior to invoking theEKF710 via theinterface704. TheEKF710, in response, performs a pose prediction in order to estimate current position data based on the vehicle motion data. TheEKF710 corrects the estimated current position data in response to the laser scanner data. Via theinterface704, the corrected current position data is communicated back to the vehicle.

FIG. 8 is a timing diagram illustrating sensor input message processing800 according to one or more embodiments. In some embodiments, various sensor devices, such as alaser scanner802, alaser scanner804 and anodometer806, within a sensor array (e.g., thesensor array108 ofFIG. 1) communicate sensor input messages to an environment basednavigation module808. Thelaser scanner802 and thelaser scanner804 may represent two dissimilar planar laser devices having different publishing rates and/or different vendors.

In order to mitigate or correct errors caused by time and motion distortion, the environment basednavigation module808 determines pose measurements in response to each acquisition time of the sensor input messages. Sensors typically provide information at the time of data acquisition internally within the device, or the time stamp is created at the time when data is made available from the sensor. Such data is subsequently communicated to software modules that form the environment basednavigation module808 for processing, where because of various data sharing techniques (e.g. serial link, Ethernet or software process) the data arrives out of time sequence when compared to other sensor data.

T₇₀₂, T₇₀₄and T₇₀₆are broadcast time periods of thelaser scanner802, thelaser scanner804 and theodometer806, respectively. δ₇₀₂, δ₇₀₄and δ₇₀₆are system delays for processing and transmitting the sensor input messages to the environment basednavigation module808. Because of different sampling periods and different system delays, the order at which the sensor data is acquired by the sensor devices is not the same as the order at which the messages became available to the environment basednavigation808. For example, a first sensor input message from thelaser scanner802 includes observed pose measurements regarding a vehicle state at an earliest time. However, this message arrives after at least one subsequent sensor input message from thelaser scanner804 and/or theodometer806, which includes observed pose measurements regarding a vehicle state at a later point in time. When the first sensor input message finally became available to theEBN808, two sensor input messages from theodometer device706 have already been made available.

In some embodiments, the publish rates (T) and/or the system delays (6) are not fixed. The environment based navigation (EBN)module808 employs a priority queue (e.g., thepriority queue310 ofFIG. 3) to address non-deterministic sensor input messages. The EBN executes a prediction-update process after processing a slowest sensor input message broadcast that is also subsequent to a prior prediction-update process. In response to an acquisition time associated with each message, theEBN module808 uses the sensor data to modify observed pose measurements. After examining each sensor input message, theEBN module808 corrects a pose prediction for the industrial vehicle.

Hence, each and every future prediction-update process is a series of filter pose prediction and update steps in which each sensor input message in the priority queue is processed in an order of acquisition time stamps (e.g., theacquisition time stamps314 ofFIG. 3). During the update step, theEBN module808 corrects a pose prediction/estimation. Alternatively, theEBN module808 fuses the sensor data to determine accurate pose measurements. For example, theEBN module808 integrates odometry data over time (i.e. dead reckoning).

As illustrated, messages from theodometer806 have a smallest system delay amongst the sensor devices, as well as a highest sampling frequency. While theodometer806 messages are inserted into the priority queue, theEBN module808 performs one or more pose prediction steps and continuously updates these vehicle pose (e.g., a current or historical pose) estimates. Then, theEBN module808 delays performance of the update step during which theEBN module808 integrates the odometry data, but does not correct the vehicle pose estimate until the update step is triggered. In some embodiments, a message from a particular type of sensor device, such as thelaser scanner802, constitutes a trigger message that initiates the update step.

As a result of the prediction-update process, theEBN module808 updates the vehicle pose estimate. In some embodiments, theEBN module808 corrects two-dimensional and three-dimensional coordinates related to vehicle location. These coordinates refer to map data associated with a shared use physical environment. In some embodiments, the vehicle pose is updated when sensor data from a trigger message becomes available to the EBN module808 (i.e., broadcast time). Upon the availability of the trigger message, theEBN module808 processes each and every sensor input message in the priority queue in the order of acquisition time. The updated vehicle pose will reflect the observed pose measurements at the time of acquisition of the trigger message.

In some embodiments, the update step is triggered before the dead reckoning error exceeds a pre-defined threshold. TheEBN module808 determines under which circumstances, the dead reckoning error is too large. For example, if the priority queue exceeds a certain length (i.e., a number of sensor input messages), sensor input message processing requires an extensive amount of time. TheEBN module808 delays the update step for a sufficient amount of time to ensure that none of the messages are processed out of order of acquisition time. In some embodiments, the update step is delayed until a sensor input message from a data source associated with a longest system delay becomes available. If such data is not received, theEBN module808 performs the update step based on acquisition time of each available sensor input message. In some embodiments, theEBN module808 deletes one or more sensor input messages if a current vehicle pose estimate has a high confidence and/or for the purpose of reducing resource workloads.

FIG. 9 illustrates a portion of the sensor input message processing900 according to one or more embodiments. Specifically, the portion of the sensor input message processing900 corresponds with the reception time (T₉₀₂) and a time correction (C₉₀₂) of thelaser scanner902.Readings910 from various sensor devices are processed, corrected and stored in aqueue912 as sensor input messages in which labels designate a source sensor device. Sensor input messages from thelaser scanner902 and thelaser scanner904 include labels “LaserA” and “LaserB”, respectively. Similarly, sensor input messages having odometry data are labeled “Odom” to indicate that theodometer906 is a source. Furthermore, the sensor input messages within thequeue912 are ordered according to acquisition time, not reception time.

A first reading is received at time t=0.5 from thelaser scanner902 and then stored in thequeue912 as a sensor input message according to an acquisition time of t=0.1, implementing thetime correction918 described above. In some embodiments, thequeue912 is rearranged such that the sensor input message is a next message to be processed instead of messages that became available earlier but were acquired at the sensor device later than the first reading. In some embodiments, anEKF914 uses the odometry data that is stored in sensor input messages having an earlier acquisition time to determine a pose prediction for time t=0.1. Because thelaser scanner902 is a trigger data source, the sensor input message is a trigger message causing theEKF914 to update the pose prediction and determine a historical pose. Odometry data that is stored in thequeue912 after the trigger message is fused and then, used to predict a current pose at time t=0.4 in view of the historical pose.

Theodometer906 publishes a second reading of odometry data at time t=0.7 and corrected with aodometry acquisition delay920 to given an acquisition time of t=0.6. As soon as the second reading becomes available to theEBN908 as a sensor input message, theEKF914 predicts a vehicle pose at time t=0.6. Then, theEBN908 stores the sensor input message associated with the third reading at the end of thequeue912. Next, a third reading from thelaser scanner904 arrives at theEBN908 and stored in thequeue912 according to acquisition time using theacquisition delay922. The third reading is not processed because thelaser scanner904 is a not a trigger data source. Subsequently, a fourth reading from theodometer906 is received, corrected and used to estimate a vehicle pose at time t=0.8. TheEBN908 integrates odometry data associated with the fourth reading is integrated with the odometry data associated with the second reading.

Finally, a fifth reading from thelaser scanner902 is processed and stored as a sensor input message in thequeue912 according to the acquisition time. Because the fifth reading has an acquisition time t=0.5, the sensor input message is inserted at a position before messages having a later acquisition time (i.e., from time t=0.6 to 0.8) and after messages having a prior acquisition time (i.e., from time t=0.1 to 0.4). Since the sensor input message is a trigger message, sensor data from the messages having the prior acquisition time is combined with laser scanner data associated with the fifth reading.

Then, the laser scanner data is used to determine pose measurements for time t=0.5 for updating the vehicle state for time t=0.1, which includes a last known vehicle state. Using odometery data from the fourth reading, theEKF914 corrects a pose prediction for time t=0.4 that is based on the last known vehicle state according to some embodiments. Lastly, theEBN908 uses the messages having the later acquisition time to forward predict a current vehicle pose at time t=0.8. In some embodiments, theEBN908 fuses odometry data stored within these messages and integrates the fused odometry data into the current vehicle pose prediction.

FIG. 10 is a functional block diagram illustrating a localization andmapping system1000 for navigating an industrial vehicle according to one or more embodiments. A plurality of sensor devices, such as laser scanner devices, provides information regarding environmental features. Readings from some of the plurality of sensor devices, such as odometers, provide a relative change in various data, such as position, velocity, acceleration and/or other vehicle motion data.

As various sensor data is communicated, a time and motiondistortion correction process1002 rearranges sensor input messages based on acquisition time, instructs aprocess1004 to extract standard features from corrected laser scanner data and stores the ordered sensor data in apriority queue1006 according to some embodiments. The extractstandard feature process1004 examines the ordered sensor data and identifies standard environmental features, which are compared to a knownfeature list1008 infilter1010. The extractstandard feature process1004 determines information regarding these environment features, such as a line, corner, arc, or marker, which are provided in a standard format for use in afilter1010. Using the ordered sensor data, thefilter1010 updates a current pose prediction for the industrial vehicle as well as map data using information regarding the environmental features as explained further below.

In some embodiments, the time and motiondistortion correction process1002 also uses vehicle motion data that corresponds with a laser scan to correct resulting laser scanner data (e.g., two-dimensional and/or three-dimensional coordinates for environmental features) in view of inaccuracies caused by motion artifacts. For example, based on a velocity parameter that is measured at or near (e.g., immediately after or before) an acquisition time of the laser scanner data, the time and motioncorrection distortion process1002 adjusts observations regarding the environmental features.

Generally, thefilter1010 provides real time positioning information for an automated type of the industrial vehicle or a manually driven vehicle. Thefilter1010 also helps provide data indicating uncertainty associated with vehicle pose measurements. Thus, should the industrial vehicle temporarily travel in an empty space without available environmental features or markers, thefilter1010 continues to provide accurate localization by updating the vehicle pose along with determining indicia of uncertainty. Thefilter1010 extracts a next sensor input message from the priority queue (e.g., a message having an earliest acquisition time) and examines information regarding the extracted standard features. The knownfeature list1008 includes a map of a physical environment. The filter810 compares selected features from the knownfeature list1008 with the extracted standard features in order to estimate vehicle position.

Depending on safety requirements, the industrial vehicle may operate within a defined degree of uncertainty with respect to a vehicle state before an error triggers analarm1014. If aprocess1012 determines that the uncertainty exceeds a predefined threshold, thealarm1014 communicates an error message to a computer, such as a mobile computer coupled to the industrial vehicle or a central computer for monitoring a physical environment. If, on the other hand, theprocess1012 determines that the uncertainty exceeds the predefined threshold, aforward prediction process1016 estimates a current vehicle state as explained further below and a publishvehicle state process1018 updates a map with information regarding the environmental features.

During the time and motiondistortion correction process1002, readings (i.e., observations) are transmitted from each sensor device. These readings may be provided by a laser and/or camera or any other type of sensor device for extracting environment features. The time and motiondistortion correction process1002 also corrects for any distortion that may be due to finite measurement time and/or speed of travel of the industrial vehicle. This distortion occurs as the industrial vehicle and sensors are moving (e.g., during a scan), which associates a temporal characteristic with data extracted from the readings.

In some embodiments, the vehicle state includes x-y coordinates associated with the vehicle location in the map as well as a vehicle heading. In some embodiments, the vehicle state includes various velocity measurements. The odometry data provides a linear velocity and a rotational velocity. The linear velocity refers to an average linear velocity of the wheels upon which encoder or other velocity measurement devices are installed. The rotational velocity is proportional to the difference between linear velocities of opposing wheels and indicates how much the heading of the vehicle has changed with respect to the global coordinate system. Thefilter1010 corrects process noise (e.g., odometry noise such as wheel slip and angular slip) by comparing the modeled motion process noise with noise from environmental observations (eg. Observations from a laser range measurement) and statistically determines a more accurate pose estimation.

Because thefilter1010 processes the sensor input messages according to acquisition time, thefilter1010 may update a vehicle state to include a vehicle pose at a point in time that is prior to a current time. As mentioned above, thefilter1010 updates the vehicle state in response to a trigger message. The updated vehicle state may be referred to as a historical vehicle state. After updating the vehicle state, theforward prediction process1014 uses odometry data corresponding with a time after an acquisition time of the trigger message to further update the historical vehicle state to include a current vehicle pose according to some optional embodiments. Prior to theforward prediction process1014, sensor input messages from the odometers are communicated to aprocess1020 for fusing the odometry data and stored in anodometry queue1022 according to some embodiments. The fused odometry data is used to execute theforward prediction process1016.

FIG. 11 is a flow diagram of amethod1100 for providing accurate localization for an industrial vehicle according to one or more embodiments. In some embodiments, an environment based navigation module (e.g., the environment basednavigation module420 ofFIG. 4) performs each and every step of themethod1100. In other embodiments, some steps are omitted or skipped. Themethod1100 starts atstep1102 and proceeds to step1104.

Atstep1104, themethod1100 initializes various sensor devices. For example, themethod1100 initializes one or more laser scanners, cameras, odometers and/or the like. Atstep1106, themethod1100 determines whether any of the sensor devices communicated a sensor input message. If sensor input is received from one of the sensor devices, themethod1100 proceeds to step1110. Otherwise, atstep1108, themethod1100 waits for a broadcast of a sensor input message. Once the sensor input message becomes available (e.g., to an environment based navigation module), themethod1100 proceeds to step1110.

Atstep1110, themethod1100 processes the sensor input message. Atstep1112, themethod1100 extracts standard features (i.e., environmental features) from the sensor input message. Atstep1114, themethod1100 attaches an acquisition time stamp to the sensor input message.

Atstep1116, themethod1100 stores the sensor input message in the priority queue. Themethod1100 rearranges sensor input messages within the priority queue according to acquisition time instead of reception time. The acquisition time, hence, for each sensor input message constitutes a priority (i.e., a value) that is used for ordering the sensor input messages. Themethod1100 determines pose measurements in response to the acquisition time associated with each sensor input message by examining a next sensor input message in the priority queue having an earliest acquisition time. In some embodiments, themethod1100 corrects a pose prediction based on the pose measurements that are observed by the sensor devices.

Atstep1118, themethod1100 determines whether a next queue entry within the priority queue includes odometry data. If the queue entry is odometry data, themethod1100 proceeds to step1120. Atstep1120, themethod1100 integrates the odometry data within the priority queue and updates a vehicle pose. If, on the other hand, the next queue entry measurement does not include the odometry data, themethod1100 proceeds to step1122. Atstep1122, themethod1100 determines whether the sensor input message was generated and communicated by a trigger data source. If the sensor input message is from the trigger data source, themethod1100 proceeds to step1124. If, on the other hand, the sensor input message is not from the trigger data source, themethod1100 returns to step1106. Atstep1124, themethod1100 performs a filter update process in order to determine accurate pose measurements and update a vehicle state. In some embodiments, themethod1100 corrects a pose prediction that is determined using the sensor data and a previous vehicle state.

Atstep1126, themethod1100 stores corrected vehicle pose in vehicle state information (e.g., thevehicle state information318 ofFIG. 3). Atstep1128, themethod1100 determines whether to terminate the localization process. If the localization process is to be terminated, themethod1100 proceeds to step1130. If the localization process is not to be terminated, themethod1100 returns to step1106. Atstep1130, themethod1100 ends.

FIG. 12 is a flow diagram of amethod1200 for updating a vehicle state for an industrial vehicle using a filter according to one or more embodiments. In some embodiments, an environment based navigation module performs each and every step of themethod1200. In other embodiments, some steps are omitted or skipped. In some embodiments, themethod1200 implements step924 of themethod1100 as illustrated byFIG. 11. Accordingly, themethod1200 is executed when a sensor input message from a trigger data source (i.e., a trigger message) is received or becomes available. Prior to performing the filter update process for the vehicle state, a filter (e.g., a process filter, such as an Extended Kalman Filter) determines a current pose prediction based on a previous vehicle state (e.g., previous vehicle pose). Themethod1200 starts atstep1202 and proceeds to step1204.

Atstep1204, themethod1200 processes a next sensor input message. In some embodiments, themethod1200 extracts the next sensor input message from a queue (e.g., a priority queue ordered by acquisition time). In some embodiments, themethod1200 examines the next sensor input message having an earliest acquisition time and extracts information regarding standard static and/or dynamic environmental features from laser scanner data. Themethod1200 also integrates any available odometry data and predicts a current vehicle pose. It is appreciated that themethod1200 generates additional information regarding the environmental features from other sensor devices, such as encoders, according to some embodiments.

Atstep1206, themethod1200 determines whether the next sensor input message is the trigger message. As explained in the present disclosure, the trigger message is communicated by the trigger data source (e.g., a particular sensor device) according to some embodiments. If the next sensor input message is also the trigger message, themethod1200 proceeds to step1208 at which pose measurement data associated with the next sensor input message is examined. In some embodiments, themethod1200 updates a pose prediction using laser scanner data and odometry data that was acquired prior to and including the trigger message.

Atstep1210, themethod1200 integrates remaining odometry data into predicting a current pose given recent vehicle movement and updating the vehicle state (e.g., thevehicle state information318 ofFIG. 3). Thestep1210 may be referred to as a forward prediction process in the present disclosure. If the sensor input message is not the trigger message, themethod1200 returns to step1204 and extracts another sensor input message from the queue in order of acquisition time. Atstep1212, themethod1200 ends.

Various elements, devices, and modules are described above in association with their respective functions. These elements, devices, and modules are considered means for performing their respective functions as described herein.

While the foregoing is directed to embodiments of the present invention, other and further embodiments of the invention may be devised without departing from the basic scope thereof, and the scope thereof is determined by the claims that follow.

Claims

1. A method for providing accurate localization at an industrial vehicle, comprising:

processing at least one sensor input message from a plurality of sensor devices, wherein the at least one sensor input message comprises information regarding environmental features;

determining pose measurements associated with the industrial vehicle in response to each acquisition time of the at least one sensor input message; and

updating a vehicle state using the pose measurements.

2. The method ofclaim 1 further comprising navigating the industrial vehicle based on the updated vehicle state.

3. The method ofclaim 1, wherein determining the pose measurements further comprises correcting the information regarding the environmental features in response to industrial vehicle movement.

4. The method ofclaim 1, wherein determining the pose measurements further comprises rearranging the at least one sensor input message according to the each acquisition time.

5. The method ofclaim 1, wherein determining the pose measurements further comprises examining the at least one sensor input message in order of acquisition time.

6. The method ofclaim 1, wherein determining the pose measurements further comprises in response to a trigger message, integrating odometry data when determining the pose measurements.

7. The method ofclaim 1, wherein updating the vehicle state further comprises updating a pose prediction using the pose measurements.

8. The method ofclaim 1, wherein updating the vehicle state further comprises delaying the updating step in response to the at least one sensor input message.

9. The method ofclaim 1 further comprising predicting a current vehicle state using odometry data and the updated vehicle state.

10. The method ofclaim 9, wherein the odometry data corresponds with an acquisition time that is after an acquisition time of a trigger message.

11. An apparatus for providing accurate localization at an industrial vehicle, comprising:

a computer coupled to the industrial vehicle and a plurality of sensors, comprising:

an environment based navigation module for processing at least one sensor input message from a plurality of sensors, wherein the at least one sensor input message comprises information regarding environmental features, determining pose measurements associated with the industrial vehicle in response to each acquisition time of the at least one sensor input message and updating a vehicle state using the determined pose measurements.

12. The apparatus ofclaim 11, wherein the environment based navigation module rearranges the at least one sensor input message according to the each acquisition time.

13. The apparatus ofclaim 11, wherein the environment based navigation module corrects the information regarding the environmental features in response to industrial vehicle movement.

14. The apparatus ofclaim 11 further comprising another computer coupled to the computer, wherein the other computer comprises a manager for communicating tasks to the computer.

15. A computer-readable-storage medium comprising one or more processor-executable instructions that, when executed by at least one processor, causes the at least one processor to:

process at least one sensor input message from a plurality of sensor devices, wherein the at least one sensor input message comprises information regarding environmental features;

determine the pose measurements associated with the industrial vehicle in response to each acquisition time of the at least one sensor input message; and

update map data associated with the modified pose measurements.

16. The computer-readable-storage medium ofclaim 15 further comprising one or more processor-executable instructions that, when executed by the at least one processor, causes the at least one processor to:

rearranging the at least one sensor input message according to the each acquisition time.

17. The computer-readable-storage medium ofclaim 15 further comprising one or more processor-executable instructions that, when executed by the at least one processor, causes the at least one processor to:

correcting the information regarding the environmental features in response to industrial vehicle movement.

18. The computer-readable-storage medium ofclaim 14 further comprising one or more processor-executable instructions that, when executed by the at least one processor, causes the at least one processor to:

storing the at least one sensor input message within a queue according to the each acquisition time.

19. The computer-readable-storage medium ofclaim 14 further comprising one or more processor-executable instructions that, when executed by the at least one processor, causes the at least one processor to:

examining the at least one sensor input message in order of acquisition time.

20. The computer-readable-storage medium ofclaim 14 further comprising one or more processor-executable instructions that, when executed by the at least one processor, causes the at least one processor to:

in response to a trigger message, integrating odometry data when determining the pose measurements.