WO2013040411A1

WO2013040411A1 - System and method for dynamic localization of wheeled mobile robots

Info

Publication number: WO2013040411A1
Application number: PCT/US2012/055511
Authority: WO
Inventors: Junmin Wang; Madhu Soodhanan GOVINDARAJAN; James W. POST, III; Andrew Fox
Original assignee: Honda Motor Co., Ltd.
Priority date: 2011-09-15
Filing date: 2012-09-14
Publication date: 2013-03-21

Abstract

A wheeled mobile robot having a laser based sensor and method for dynamically correcting the estimate of the Extended Kalman Filter, used in determining a position and orientation of the WMR based on data representing a measurements effectuated by the laser-based sensor. The dynamic correction of the estimate of the EKF does not require a priori knowledge of the medium surrounding the WMR.

Description

SYSTEM AND METHOD FOR DYNAMIC LOCALIZATION

OF WHEELED MOBBLE ROBOTS

CROSS REFERENCE TO RELATED APPLICATIONS

[0001] This application claims the benefit of and priority form the U.S. Provisional Patent

Application No. 61/535,033 filed on September 15, 2011 and titled "Extended Kalman Filter Based Dynamic Localization of Wheeled Mobile Robots Using a Laser Sensor," the entire contents of which are hereby incorporated by reference for all purposes.

TECHNICAL FIELD

[0002] The present invention relates to means for localization of wheeled robots and, more particularly, to an Extended Kalman Filter (EKF) based dynamic localization of a wheeled mobile robot (WMR) with the use of a laser sensor.

BACKGROUND ART

[0003] Localization is an important problem in the field of mobile robotics. When a wheeled mobile robot (WMR) is enabled with the capability to ascertain its own pose (including orientation and position), it is enabled to perform autonomous tasks. Localization can be generally performed using, for example, a) internal sensors such as odometer or a wheel speed encoder, or b) an external sensor such as a video camera, a laser sensor, an ultrasonic sensor, and a radio frequency (RF) identification sensor. Localization based on a global-positioning system (GPS) methodology is primarily carried out as sensor fusion technique, due to the relatively low accuracy and update rate of the GPS. The accuracy of localization that utilizes internal sensors may not be necessarily sufficient because the posture error and the uncertainties that are inherent to internal sensors accumulate, in operation. A laser sensor is one of the preferred external sensors for WMR applications because the operation of such sensor does not depend on lighting conditions of the ambient environment (as compared, for example, with a video camera). In addition, and in contradistinction with an ultrasonic sensor, a laser sensor is less sensitive to environmental noise. [0004] Conventionally, implementation of a laser sensor based localization approach and system requires the knowledge of characteristics of ambient environment, which may sufficiently limit the breadth of such technique when feedback data representing the ambient conditions are not readily available. In order to improve the versatility of the laser sensor based localization methodology, it is desired, therefore, to decouple such methodology from a need for the knowledge of the operational environment.

SUMMARY QF THE INVENTION

[0005] Embodiments of the present invention provide an article of manufacture comprising a microprocessor; and a computer readable medium. The computer readable medium includes comprising computer readable program code disposed thereon for determination of positioning and orientation of a wheeled mobile robot (WMR) that is operably associated with the processor, that has a laser based sensor disposed thereon, and that is characterized by a longitudinal velocity and geometrical parameters. In a specific implementation the geometrical parameters include a wheel base and a length of the WMR. The computer readable program code contains a series of computer readable program steps configured to effect (i) determining coordinates of position and angular orientation of the WMR in space at a second time instant by defining a state function of coordinates of position and angular orientation of said WMR in space at a first time instant, where the first time instant preceding the second time instant; and (ii) determining a prediction estimate of Extended Kalman Filter (EKF) using a partial derivative of so defined state function. The computer readable program code additionally contains computer readable program steps adapted to effect (iii) calculating a correction update of the EKF; and (iii) identifying a corrected state estimate of said EKF.

[0006] In one embodiment, the step of deteraiining the coordinates includes determining coordinates based on data representing spatial information associated with the WMR and acquired by the laser based sensor. Alternatively or in addition, the step of determining the coordinates may include determining the coordinates based on data representing said longitudinal velocity of the WMR and said geometrical parameters of the WMR. In a specific implementation, the step of determining the coordinates may include includes determining the coordinates based on data representing a wheel base and a length of said WMR. Alternatively or in addition, the step of determining a prediction estimate, effectuated by the microprocessor may includes determining a prediction estimate with the use of partial derivatives of the state function at least with respect to coordinates of position and angular orientation of the WMR at the first time instant.

[0007] In one embodiment, the article of manufacture contains a laser based sensor that is operable to scan a space adjoining the WMR within an angular span and during a scanning period, and a microprocessor adapted to effectuate a calculation of a correction update at an end of said scanning period of the laser based sensor. In a related embodiment, the microprocessor is adapted to identify a corrected state estimate of the EKF by modifying the prediction estimate of the EKF with the previously calculated correction update of the EKF.

[0008] Embodiments of the present invention further provide a method for moving a wheeled mobile robot (WMR) that has a laser based sensor disposed thereon and operable to scan a space adjoining the WMR within an angular span and during a scanning period, and that is characterized by a longitudinal velocity and geometrical parameters. The method includes the steps of (i) deten ining coordinates of position and angular orientation of the WMR in space at a second time instant by defining a state function of coordinates of position and angular orientation of the WMR in space at a first time instant that precedes the second time instant; (ii) determining a prediction estimate of Extended Kalman Filter (EKF) using a partial derivative of so defined state function at least with respect to coordinates of position and angular orientation of the WMR at the first time instant; (iii) calculating a correction update of the EKF corresponding to an end of the scanning period; and (iv) identifying a corrected state estimate of the EKF. In one implementation, each of the steps of determining coordinates, determining a prediction estimate, and calculating a correction update is devoid of using data representing an environment that surrounds the WMR. Identification of a corrected state estimate may include forming a data set representing spatial position and orientation of the WMR and including the prediction estimate, of the EKF, that is modified with the correction update of the EKF. The method may optionally include, in addition, receiving (from the laser based sensor and by a processor operably associated with the WMR) data representing measurements of spatial coordinates and orientation of the WMR effectuated with the use of the laser based sensor in order to determine, at the end of said scanning period, parameters of localization of spatial positioning and orientation of the WMR in global system of coordinates. In a specific case, the method may further include a step of docking the WMR at a predetermined location based on so determined parameters of localization. In a related embodiment, the step of identification of a corrected state estimate of the EKF includes identifying a corrected state of said EKF at the end of the scanning period associated with operation of the laser based sensor.

[0009] Embodiments of the invention further provide a wheeled mobile robotic (WMR) system that contains (i) a laser based sensor disposed thereon (and operable to scan a space adjoining the WMR system within an angular span and during a scanning period) and that is characterized by a longitudinal velocity and geometrical parameters, and (ii) a processor in operable communication with the laser based sensor. The processor is configured to

determine coordinates of position and angular orientation of the WMR system in space at a second time instant by defining a state function of coordinates of position and angular orientation of the WMR system in space at a first time instant, the first time instant preceding the second time instant;

determine a prediction estimate of Extended Kalman Filter (EKF) using a partial derivative of said state function; and

identify a corrected state estimate of the EKF by modifying the prediction estimate with a correction update that is calculated at the end of the scanning period based on data representing measurements of spatial coordinates and orientation of the WMR system, as effectuated with the use of the laser based sensor.

[0010] In one embodiment, the processor of the WMR system is programmed to identify a corrected state estimate of the EKF without the use of data representing an environment surrounding the WMR system. Alternatively or in addition, the processor may be programmed to determine a prediction estimate of the EKF with the use of partial derivatives of the state function with respect to (i) global coordinates, (ii) noise of scanning the space adjoining the WMR system with the laser based sensor, and (iii) statistical parameters related to distributions of longitudinal velocity and a steering angle of the WMR. In a related implementation, the geometrical parameters of the WMR system include a length and wheel base of the WMR system. The processor may be additionally programmed to acquire data representing measurements of spatial coordinates and orientation of the WMR system as effectuated with the use of the laser based sensor in order to determine, at the end of scanning period, parameters of localization of spatial positioning and orientation of the WMR system in global system of coordinates. Moreover, the processor of the WMR system is optionally programmed to determine parameters of localization of spatial positioning and orientation of the WMR system in global system of coordinates based at least on WMR system dimensions, positioning, and orientation in local system of coordinates, as well as data representing noise of said measurements.

BRIEF DESCRIPTION OF THE DRAWINGS

[0011] The invention will be more fully understood by referring to the following Detailed

Description in conjunction with the Drawings, of which:

Fig. 1 is a diagram showing a cross-section of an example target according to an embodiment of the invention;

Fig. 2 is a diagram illustrating the WMR position and orientation in global coordinates, the WMR having a system of local coordinates;

Fig. 3 is a plot illustrating longitudinal velocity of the WMR moving along a straight line;

Fig. 4 is a plot showing a steering input to the WMR moving along a straight line;

Fig. 5 is a plot illustration the results of localization, carried out according to an embodiment of the invention with scanning a 180 degree span in about 4 seconds;

Fig. 6 is a plot illustration the results of localization, carried out according to an embodiment of the invention with scanning a 180 degree span in about 2 seconds;

Fig. 7 is a plot illustration the results of localization, carried out according to an embodiment of the invention with scanning a 180 degree span in about 1 second;

Fig. 8 is a plot illustrating the error in determination of the WMR localization along the x-axis according to a dynamic model and according to an embodiment of the invention, for the WMR moving along the straight line;

Fig. 9 is a plot illustrating the error in determination of the WMR localization along the y-axis according to a dynamic model and according to an embodiment of the invention, for the WMR moving along the straight line;

Fig. 10 is a plot showing the error in determination of the WMR orientation determined with the use of a dynamic model and with the use of an embodiment of the invention, for the WMR moving along the straight line; Fig. 11 is a plot illustrating longitudinal velocity of the WMR making an S-turn or performing a lane-change maneuver;

Fig. 12 is a plot showing a steering input to the WMR performing an S-turn and a lane-change maneuver;

Fig. 13 is a plot illustration the results of localization, carried out according to an alternative embodiment of the invention with scanning a 180 degree span in about 4 seconds;

Fig. 14 is a plot illustration the results of localization, carried out according to an alternative embodiment of the invention with scanning a 180 degree span in about 2 seconds;

Fig. 15 is a plot illustration the results of localization, carried out according to an alternative embodiment of the invention with scanning a 180 degree span in about 1 second;

Fig. 16 is a plot illustrating the error in determination of the WMR localization along the x-axis according to a dynamic model and according to an embodiment of the invention, for the WMR making an S-turn or performing a lane-change maneuver;

Fig. 17 is a plot illustrating the error in determination of the WMR localization along the y-axis according to a dynamic model and according to an embodiment of the invention, for the WMR making an S-turn or performing a lane-change maneuver;

Fig. 18 is a plot showing the error in determination of the WMR orientation determined with the use of a dynamic model and with the use of an embodiment of the invention, for the WMR making an S-turn or performing a lane-change maneuver;

Fig. 19 is a flow-chart representing an embodiment of the method of the invention.

DETAILED DESCRIPTION

[0012] In accordance with preferred embodiments of the present invention, methods and apparatus are disclosed for a system and method for localization of a wheeled mobile robot with the use of a laser sensor, which system and method do not require a priori knowledge of ambient environment. [0013] The presently used methods for localization of wheeled robots suffer from various deficiencies that relate to the nature of a sensor used for the purposes of such localization. For example, the use of RF-identification sensors requires knowledge of characteristics of ambient environment and, therefore, if data representing such characteristics are not available the operation of the RFID sensors is impaired. As another example, the GPS based localization cannot be used with the WMR without the availability of a local GPS base station, making such localization methodology rather expensive.

[0014] External sensor based localization techniques have also been used to update a local map in a global environment which is called Simultaneous Localization and Mapping (SLAM) technique. In SLAM, the sensor information is used to improve localization results by comparing a local map (obtained from the latest scan) and an already available global map. Related art considered several approaches and algorithms involved in solving the SLAM problem.

[0015] In accordance with embodiments of the present invention, a static localization technique is extended to a case of dynamic localization. In reference to Fig. 1 , an onboard laser sensor scans continuously the space defined in front the sensor within an angular range of about 180 degrees. Once the localization algorithm recognizes the unique target 100 at a known global location, laser sensor measurements along with the rotary encoder position are carried out to localize the WMR globally.

[0016] In reference to various figures below, the following nomenclature is used:

/ denotes the length of a target; x_v,y_v define the body-fixed coordinates (an imaginary co-ordinate system with the origin placed on top of the laser sensor); represents the angle made by the laser sensor to the left end of the target with the respect to the positive χ_ν axis; represents the angle made by the laser sensor to the right end of the target with respect to the positive χ_ν axis; the values of mm mm have respectively corresponding meanings representing measurements according to the same convention as the values of angles defined above; u_s represents a unit step function; x,y are the global coordinates of the WMR; φ defines the WMR orientation angle defined by the localization procedure; a denotes a wheelbase for the WMR; V, \^'s the longitudinal velocity of the WMR; and y\^'s the steering angle of the WMR. (I have improved this paragraph to explain the nomenclature) [0017] Though the laser scanning process is continuous, localization results are typically obtained only at the end of every scan, which takes some amount of time depending on the rotary stage capabilities and the laser sensor accuracy. For example, the accuracy of the Leuze laser sensor can be improved with repeated measurements performed at the same spot (which requires an increase of the scanning period). The estimated least amount of scanning time required for the entire 180 degree space span based on this information for a WMR considered in this application is about 1 second. In order to overcome the limitations of the static measurement and make the dynamic localization possible, it can be used in conjunction with localization based on internal sensors.

[0018] The dynamic localization issue is often addressed by using a so-called Extended

Kalman Filter (EKF) because of its convergence properties. The convergence properties of the EKF significantly depend on estimating the process and measurement noise covariance matrices. The localization procedure carried out with the use of the EKF has also been referred to as a sensor fusion technique.

[0019] Embodiments of the present invention demonstrate that accumulating, throughout the operation of the internal sensor, the noise of the internal sensor can be minimized with the use of laser sensor based approach to localization measurements. The prediction step of the EKF is performed by means of simulating a kinematic model of the WMR The process noise covariance matrix includes the standard deviation values associated with a wheel speed sensor and a steering wheel input. The step of EKF correction is performed by using the laser sensor based localization results. In the prediction step, associated with the use of the EKF, a simple kinematic model of a 1/10 scale front wheel steered car-like mobile robots is used. The localization equations - and, in particular, Eqs. 7 and 9, derived by Govindarajan et al. in "Design and Analysis of a Localization Method Using a Laser sensor for Indoor Wheeled Mobile Robots", Proc. ASME Dynamic Systems and Control Conf, Washington, D.C. (2011) - are used here as the measurement equations in the correction step. According to the embodiment of the invention, at the correction step of the EKFG algorithm, in order to perform the localization procedure only the results of the laser sensor measurements and angular measurements obtained in relation to the vehicle and target are used, which do not include any measurement results associated with ambient medium.

Prediction Step of EKF [0020] According to an embodiment of the invention, and in reference to Fig. 2, the orientation or azimuth angle ' φ ' of the WMR and its location in the global xy system of coordinates are predicted by simulating the kinematic model expressed as

Eq. (1)

For low speed applications, the lateral velocity of the vehicle is assumed to be substantially negligible because the WNR is known to exhibit no side slip at low speeds.

[0021] Re- ritten in a discrete form, the continuous kinematic model of E . 1 becomes

which, functionally, can be expressed as follows:

Eq. (3)

[0022] In Eq. (3), X_k represents the WMR's position and orientation about the global coordinates. The noise vector w_k__x is a collection of the standard deviation values corresponding to the wheel speed sensor and the steering input. Sensor noise distributions are assumed to be in a Gaussian form with the zero mean, and ΔΤ denotes the sampling rate.

[0023] The prediction step of the EKF is carried out according to pk = F_k_ P_k__x ⁺F _k_ + L_k_ Q_k__x ⁺L _k__x

where [F] , [L] , and [Q] matrices are, respectively, Jacobian of the state equation with respect to the states, Jacobian of the state equation with respect to the noise variables, and the process noise covariance matrix. When linearized, the [F], [L] and [Q] matrices can be expressed as

where σ² is a variance. The initiation of the EKF requires the knowledge of precise global position and orientation. Accordingly, the filter can be initiated at a k=0 instant of time as follows, based on results obtained with the laser-sensor based set of measurements:

P₀ ⁺ = E[(X₀ -X₀ ⁺){X₀ -X₀ ⁺)' ] Eq. (6)

Correction Step of EKF

[0024] As discussed above, the laser sensor based localization results and/or the

measurements for the states in the kinematic model are available at the end of scanning period. The laser sensor based localization equations, which include, according to Govindarajan et al,

are further used, according to an embodiment of the invention, as measurement equations for the states of the kinematic model

z_k = k_laser_avail Eq. (8)

[0025] In reference to Eqs. (8) and (9), k = k_ laser _ avail are the instants of time when laser sensor based localization results are available, and k - k _ kin are those instants of time when laser sensor results are not available, and

[0026] Because the laser sensor measurement is available only at the end of a scanning period, a skilled artisan will appreciate that a correction step is performed only at instants k = k_ laser _ avail . The noise vector v_k is a collection of the standard deviations of the noise values corresponding to the laser sensor measurement and the rotary stage encoder measurement. The noise distributions of the respective sensors are assumed to be in a form of a Gaussian with a zero mean.

[0027] The measurement-based correction update of the state estimate and estimation error covariance are determined according to

K_k = P_k-H_k ^T{H_kP-H_k ^T +M_kR_k-M_k ^r

P = (I- K_kH_k)p- Bq. (10). X_k ⁺ = X-_{k +} K_k[z_k -h_k(X_k-,0)]

where H and M matrices are, respectively, Jacobian of the measurement equation with respect to the measurements and Jacobian of the measurement equation with respect to the noise variables. [Η] and [M] are expressed as dh_t _ dh_k

„ = , M Eq. (11) dx dv

[0028] Because the step of correction of the EKF is also available only at the end of every scanning period, the overall state estimate of the EKF can be expressed as

[0029] A person skilled in the art will readily appreciate that the prediction step results are used when the correction step cannot be performed (that is, at k = k _kin) and the correction step results are used at k = k _ laser _ avail .

[0030] Fig. 19 is a flow-chart schematically representing an embodiment of the method and/or algorithm of the invention. Here, at step 1900, a manner is established in which a set of parameters defining the instantaneous positioning and orientation of a target (such as the WMR, for example) is determined based on a set of parameters defining the immediately preceding positioning and orientation of the target. In particular, descriptors or values representing current angular orientation and positioning of the target in global system of coordinates is determined, with the use of the kinematic model, as a state function of the values representing immediately preceding global coordinates, velocity, and dimensional parameters of the target. In a specific case, one of the variables of such state function includes measurement noise. Calculations corresponding to step 1900 can be carried out with the use of, for example, Eqs. (1) and (2).

[0031] At step 1920, predictions for EKF are made with the use of partial derivatives of the state function, determined in step 1910, with respect to global coordinates, measurement noise, and statistical parameters related to distributions of longitudinal velocity and steering angle of the target. In a specific implementation, as represented by Eqs. (4) and (5) such prediction includes solving a matrix-based algebraic equation. At step 1930, data corresponding to the laser sensor

measurements are received by a system to determine localized parameters describing target positioning and orientation in the global system of coordinates. The data received by the system represent the laser scan defined by a predetermined scan angle in front of the target and a predetermined scanning period and are received at the end of the predetermined scanning period. In a specific case, the determination of the localized parameters is performed with the use of Eqs. (7-9) and at least in partial reliance on target dimensions, positioning, and orientation of target in local system of coordinates, as well as measurement noise figure. At step 1940, correction update(s) are calculated for the EKF at time instants corresponding to end of each of scanning periods during which the laser sensor based measurements were performed at step 1930. In particular, the calculation may be carried out with the use of a matrix-based algebraic expression including a Jacobian matrix of a measurement equation with respect to the measurements and a Jacobian matrix of a measurement equation with respect to the noise parameters, as shown in Eqs. (10-11).

Following step 1940, an overall corrected state estimates of the EKF are defined corresponding to each of the scanning periods during which the laser sensor based measurements were performed. Such definition is provided at step 1940 and can be effectuated without the use of knowledge of the environment that is ambient to the target.

Examples of Implementation of an Embodiment

[0032] A couple of examples of practical implementations of embodiment of the invention are discussed below. In these examples, the following parameters were used for calculations:

[0033] For the prediction step of EKF (step 1920 of Fig., 19), an estimate of the process noise parameters include

and the Jacobian matrices of Eq. Eq. (5) include

[0034] For the correction step of the EKF (step 1940 of Fig. 19), an estimate of the process noise parameters is carried out according to

and the Jacobian matrices of Eq. Error! Reference source not found, are rewritten as

ivi _k - dv

because the measurement equation has a linear dependence on the states and measurement noise. In other words, the H and M matrices include identity matrices.

[0035] Embodiment Involving Movement of the WMR Along a Straight Line. A simulation pertaining to an WMR, which is assumed to move along a straight line at a speed of 0.5 m/s for 10 seconds, was carried out using both the EKF and the considered kinematic model. For the correction step of the EKF, the accuracy of the results depends on the scanning period of the laser sensor. Accordingly, the simulations were performed for different scanning periods (4s, 2s, and Is) to ascertain the accuracy of the results of the WMR localization. In these simulations, and in reference to Fig. 3 showing the determined longitudinal velocity of the WMR as a function of time, noise having Gaussian distribution was added to the longitudinal velocity. Fig. 4 shows the steering input to the vehicle, with the added Gaussian noise.

[0036] The results of the localization procedure obtained using an EKF-based determination and those obtained using a dynamic model estimation are presented in Figs. 5, 6, and 7 for the scanning periods of about 4 seconds, about 2 seconds, and about 1 second, respectively, in comparison with the (assumed) actual motion of the vehicle. A skilled artisan will appreciate that localization of the moving WMR achieved using the EKF estimation for the shortest scanning period (of about 1 second) of Fig. 7 is sufficiently accurate in that the deviation between the localized coordinates (x, y) and the actual coordinates of the WMR are substantially negligible. [0037] To better ascertain the performance of an embodiment of the EKF-based localization methodology at the scanning speed corresponding to Fig. 7, the errors in determination of the localization results (with respect to x coordinate, y coordinate, and orientation) are compared with the corresponding errors accompanying a localization procedure carried out with the use of the kinematic model. Plots representing such comparison are shown in Figs. 8, 9, and 10, respectively. The deterrnined angular orientation of the WMR is within about 0.02 degrees from the actual azimuthal orientation and each of the determined localized coordinates x and v is within less than about 0.5 mm from the corresponding actual coordinate. In comparison, the errors accompanying the WMR localization with the use of a kinematic model are rather high: on the order of 0.2 degrees in angular orientation (see Fig. 10), on the order of 5 mm along the x-axis (see Fig. 8), and on the order of 0.5 mm along the y-axis (see Fig. 9). Accordingly, the use of the EKF-based methodology is advantageous over the kinematic model approach for scanning speeds at which a period of scanning of the entire frontal area of 180 degrees is about 1 second.

[0038] Embodiment Involving Movement of the WMR According to an S-turn or a Lane-

Change Maneuver. A simulation pertaining to an WMR assumed to perform an S-turn or a lane- change maneuver, lasting for 10 seconds at a speed of 0.5 m/s, was carried out using both the EKF and the considered kinematic model. The simulations were performed for different scanning periods (4s, 2s, and Is) to ascertain the accuracy of the results of the WMR localization, and noise having Gaussian distribution was added to the longitudinal velocity of the WMR, as illustrated in Fig. 11. Error! Reference source not found.12 shows the steering input to the vehicle, with the added Gaussian noise.

[0039] In a fashion similar to that discussed in reference to Figs. 5, 6, and 7, the results of the localization procedure obtained using an EKF-based determination and those obtained using a dynamic model estimation are presented in Figs. 13, 14, and 15 for the scanning periods of about 4 second, about 2 seconds, and about 1 second, respectively, in comparison with the (assumed) actual motion of the vehicle. A skilled artisan will appreciate that localization of the moving WMR achieved using the EKF estimation for the shortest scanning period (of about 1 second) of Fig. 15 is sufficiently accurate in that the deviation between the localized coordinates (x, y) and the actual coordinates of the WMR are substantially negligible.

[0040] To better ascertain the performance of an embodiment of the EKF-based localization methodology at the scanning speed corresponding to the scanning period of 1 second of Fig. 15, the errors in determination of the localization results (with respect to x coordinate, y coordinate, and orientation) are compared with the corresponding errors accompanying a localization procedure carried out with the use of the kinematic model. Plots representing such comparison are shown in Figs. 16, 17, and 18, respectively. The determined angular orientation of the maneuvering WMR is within about 0.05 degrees from the actual azimuthal orientation and each of the determined localized coordinates x and v is within less than about 0.5 mm from the corresponding actual coordinate. In comparison, the errors accompanying the WMR localization with the use of a kinematic model are rather high: on the order of 0.45 degrees in angular orientation (see curve a of Fig. 18), on the order of 11 mm along the j -axis (see Fig. 16), and on the order of 0.3 mm along the _ -axis (see Fig. 17). Accordingly, the use of the EKF-based methodology is confirmed to be advantageous over the kinematic model approach when considering the WMR maneuvering to accomplish the S-turn or to effectuate a lane change for period of scanning, of the entire frontal area of 180 degrees, of about 1 second.

[0041] Embodiments of the invention were disclosed that are directed to the use of an EKF- based dynamic localization technique, for WMR, that utilizes a laser sensor and wheel speed encoders. The laser sensor is used to recognize the pattern of a unique target, which, in turn, is used as the reference to localize the WMR globally. When implementing the EKF, a prediction step is performed by using the kinematic model alone, while the correction step is performed using the laser sensor based localization technique. The localization results confirm that the proposed dynamic localization technique can be adopted for high accuracy target localization applications.

[0042] Effectuation of the steps of a method of the invention may require the operation of a processor controlled by instructions stored in a tangible memory element. The memory may be random access memory (RAM), read-only memory (ROM), flash memory or any other memory, or combination thereof, suitable for storing control software or other instructions and data. Some of the functions that can ne performed by the processor have been described with reference to a flowchart . Those skilled in the art should readily appreciate that functions, operations, decisions, etc. of all or a portion of each block, or a combination of blocks, of the flowcharts or block diagrams may be implemented as computer program instructions, software, hardware, firmware or combinations thereof. Those skilled in the art should also readily appreciate that instructions or programs defining the functions of the present invention may be delivered to a processor in many forms, including, but not limited to, information permanently stored on non-writable storage media (e.g. read-only memory devices within a computer, such as ROM, or devices readable by a computer I/O attachment, such as CD-ROM or DVD disks), information alterably stored on writable storage media (e.g. floppy disks, removable flash memory and hard drives) or information conveyed to a computer through communication media, including wired or wireless computer networks. In addition, while the invention may be embodied in software, the functions necessary to implement the invention may optionally or alternatively be embodied in part or in whole using firmware and/or hardware components, such as combinatorial logic, Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs) or other hardware or some combination of hardware, software and/or firmware components.

[0043] While the invention is described through the above-described exemplary embodiments, it will be understood by those of ordinary skill in the art that modifications to, and variations of, the illustrated embodiments may be made without departing from the inventive concepts disclosed herein. Accordingly, the invention should not be viewed as being limited to the disclosed embodiment(s).

Claims

CLAIMS What is claimed is:

1. An article of manufacture comprising

a microprocessor; and

a computer readable medium comprising computer readable program code disposed thereon for determination of positioning and orientation of a wheeled mobile robot (WMR) operably associated with said processor and having

a laser based sensor disposed thereon,

a longitudinal velocity, and

geometrical parameters,

the computer readable program code comprising a series of computer readable program steps to effect

determining coordinates of position and angular orientation of said WMR in space at a second time instant by defining a state function of coordinates of position and angular orientation of said WMR in space at a first time instant, the first time instant preceding the second time instant;

determining a prediction estimate of Extended Kalman Filter (EKF) using a partial derivative of said state function;

calculating a correction update of said EKF; and

identifying a corrected state estimate for said EKF.

2. An article of manufacture according to claim 1, wherein said determining coordinates includes determining coordinates based on data representing spatial information associated with the WMR and acquired by the laser based sensor.

3. An article of manufacture according to claim 1, wherein said determining coordinates includes determining coordinates based on data representing said longitudinal velocity of the WMR and said geometrical parameters of the WMR.

4. An article of manufacture according to claim 2, wherein said determining coordinates includes determining coordinates based on data representing a wheel base and a length of said WMR.

5. An article of manufacture according to claim 1 ,

wherein said determining a prediction estimate includes determining a prediction estimate with the use of partial derivatives of said state function at least with respect to coordinates of position and angular orientation of the WMR at the first time instant.

6. An article of manufacture according to claim 1, wherein said laser based sensor is operable to scan a space adjoining said WMR within an angular span and during a scanning period, and wherein said calculating correction updates includes calculating correction updates at an end of said scanning period.

7. An article of manufacture according to claim 1 , wherein said identifying a corrected state estimate includes modifying said prediction estimate of said EKF with said correction update of said EKF.

8. A method for moving a wheeled mobile robot (WMR) having

a laser based sensor disposed thereon and operable to scan a space adjoining said WMR within an angular span and during a scanning period,

a longitudinal velocity, and

geometrical parameters,

the method comprising:

determining coordinates of position and angular orientation of said WMR in space at a second time instant by defining a state function of coordinates of position and angular orientation of said WMR in space at a first time instant, the first time instant preceding the second time instant; determining a prediction estimate of Extended Kalman Filter (EKF) using a partial derivative of said state function at least with respect to coordinates of position and angular orientation of the WMR at the first time instant; calculating a correction update of said EKF corresponding to an end of said scanning period; and

identifying a corrected state estimate of said EKF.

9. A method according to claim 8, wherein each of said determining coordinates, determining prediction estimate, and calculating a correction update is devoid of using data representing an environment that is ambient with respect to the WMR.

10. A method according to claim 8, wherein said identifying a corrected state estimate includes forming a data set representing spatial position and orientation of said WMR and including said prediction estimate of the EKF modified with said correction update of the EKF.

11. A method according to claim 8, further comprising receiving, from said laser based sensor and by a processor operably associated with said WMR, data representing measurements of spatial coordinates and orientation of the WMR effectuated with the use of said laser based sensor to determine, at the end of said scanning period, parameters of localization of spatial positioning and orientation of the WMR in global system of coordinates.

12. A method according to claim 11 , further comprising docking the WMR at a

predetermined location based on so determined parameters of localization.

13. A method according to claim 8, wherein said identifying includes identifying a corrected state of said EKF at the end of said scanning period.

14. A wheeled mobile robot (WMR) system comprising:

a laser based sensor disposed thereon and operable to scan a space adjoining said WMR system within an angular span and during a scanning period,

a longitudinal velocity,

geometrical parameters, and

a processor in operable communication with the laser based sensor, said processor being programmed to determine coordinates of position and angular orientation of said WMR system in space at a second time instant by defining a state function of coordinates of position and angular orientation of said WMR system in space at a first time instant, the first time instant preceding the second time instant;

identify a corrected state estimate of said EKF by modifying said prediction estimate with a correction update that is calculated at the end of the scanning period based on data representing measurements of spatial coordinates and orientation of the WMR system effectuated with the use of said laser based sensor.

15. A WMR system according to claim 16, wherein the processor is programmed to identify a corrected state estimate of the EKF without the use of measurement data representing an environment surrounding the WMR system.

16. A WMR system according to claim 16, wherein the processor is programmed to determine a prediction estimate of the EKF with the use of partial derivatives of the state function with respect to (i) global coordinates, (ii) noise of scanning the space adjoining the WMR system with the laser based sensor, and (iii) statistical parameters related to distributions of longitudinal velocity and a steering angle of the WMR system.

17. A WMR system according to claim 16,

wherein the geometrical parameters include a length and wheel base of the WMR system, and

wherein the processor is further programmed to acquire, from the laser based sensor, data representing measurements of spatial coordinates and orientation of the WMR system

effectuated with the use of said laser based sensor in order to determine, at the end of said scanning period, parameters of localization of spatial positioning and orientation of the WMR system in global system of coordinates.

18. A WMR system according to claim 17, wherein said processor is configured to a determine parameters of localization of spatial positioning and orientation of the WMR system in global system of coordinates based at least on WMR system dimensions, positioning, and orientation in local system of coordinates, as well as data representing noise of said

measurements.