Abstract:
A mobile robot and a method of localizing the robot are disclosed. The robot includes a gyroscope module providing information regarding a rotational angle of a gyroscope; an encoder module providing information regarding velocity and information regarding a rotational angle of a wheel of the robot by sensing motion of the wheel; and a control module estimating a current pose of the robot according to a method based on information provided by the encoder module and the gyroscope module, the control module incorporating information regarding rotational angle provided by the gyroscope module when estimating the current pose.

Description:
CROSS-REFERENCE TO RELATED APPLICATIONS  
       [0001]     This application claims the benefit of Korean Patent Application No. 10-2005-0106088 filed on Nov. 7, 2005, in the Korean Intellectual Property Office, the disclosure of which is incorporated herein by reference in its entirety.  
       BACKGROUND OF THE INVENTION  
       [0002]     1. Field of the Invention  
         [0003]     The present invention relates to a method of localizing a robot. More particularly, the present invention relates to a method of localizing a mobile robot by using a Kalman filter with constrains as well as to a robot adapted to localize itself using this method.  
         [0004]     2. Description of the Related Art  
         [0005]     Various types of robots have recently appeared as a result of technological advancement. Particularly, there are robots capable of moving inside a house by themselves in order to do household labor. Self-localization of mobile robots is very important, and many methods have been proposed to accomplish this feature. In order to localize a robot, it is customary to attach an encoder (or odometer) to a driving unit (e.g., a wheel) of the robot or mount a gyroscope onto the robot in order to measure the correct rotational angle of the robot. However, slippage or mechanical drift of a wheel, when a robot is localized, causes error. Although such error may be negligible at first, it may accumulate and cause significant problems.  
         [0006]     A Kalman filter algorithm is conventionally used to localize a robot. However, due to the absence of constraints, estimated parameters often violate physical constraints. For example, when the wheel base distance of a mobile robot is W b  and the physical range of W b  is 0.3 m≦W b ≦0.4 m, the estimated W b  may be outside this range. In the case of SLAM (simultaneous localization and mapping), which uses ceiling vision based on the Kalman filter, the estimated ceiling height z must be in a certain range, e.g., 2.4 m≦W b ≦2.5 m. However, the estimated z may be outside this range.  
         [0007]     A conventional Indirect Kalman filtering method, proposed by Chung et al., uses a combination of an odometry error model (or encoder error model) and a gyroscope angular error model as a process model, and uses the measured angle difference between a gyroscope and an odometer as the measurement model. The indirect Kalman filter is described in detail in “Accurate Mobile Robot Dead-reckoning With a Precision-calibrated Fiber-optic Gyroscope,” by H. Chung, L. Ojeda, and J. Borenstein in IEEE Transactions on Robotics and Automation, vol. 17, Issue 1, pp.80-84, February 2001. The state equation of the conventional indirect Kalman filter method includes various parameter estimates, such as error in robot pose estimated by the encoder, error in robot angle estimated by the gyroscope, scale factor of the odometry, and wheel tread distance, as well as the state of various sources of noise, as shown in  FIG. 3 .  
         [0008]     In  FIG. 3 , state variables labeled  310  refer to error in robot pose estimated by the encoder; state variables labeled  320  refer to error in robot heading angle estimated by the gyroscope; state variables labeled  330  refer to various parameter estimations; and a state variable labeled  340  refers to noise.  
         [0009]     In  FIG. 3 , S R (k) and S L (k) refer to the scale factor errors of the right and left encoders, respectively, and D(k) refers to the tread error, i.e., the error introduced by variations in the distance between the left and right wheels.  
         [0010]     However, an indirect Kalman filter based on the state equation shown in  FIG. 3  has poor observability. Particularly, although the state vector in the state equation of  FIG. 3  has a dimension of 9, the rank of the observability matrix is merely 2 under an assumption that the state transition matrix is in a steady state of a time-invariant system.  
         [0011]     The fact that the state of the robot is not fully observable makes it impossible to guarantee whether or not remaining states converge. More particularly, since nothing but a rotational angle state is included in the observation matrix, it cannot be confirmed whether or not a state, such as error in position (x, y) of the robot, converges to a true value. Even when the system is in a time-varying condition, the observability matrix has a rank of 6 and is still unstable.  
         [0012]     Therefore, in order to solve this problem, the present invention introduces an additional observable system state for estimation.  
         [0013]     Therefore, it is also necessary to both reduce error in localizing a robot so that the stability of the robot is guaranteed, and make parameters estimated when applying a Kalman filter satisfy physical constraints so that the performance of the robot is improved.  
         [0014]     A measurement equation according to the related art is defined by equation (6) below and shown in  FIG. 6 . It is clear from  FIG. 6  that only an angular state is included in the observation matrix H. 
 
Measurement equation=orientation of robot estimated by encoder−orientation of robot estimated by gyroscope+noise   (6) 
 
 In  FIG. 6 , “v” refers to measurement noise. 
 
       SUMMARY OF THE INVENTION  
       [0015]     Accordingly, the present invention has been made to solve the above-mentioned problems occurring in the related art, and an aspect of the present invention is to provide a method of localizing a robot wherein localization error is reduced by adjusting the rank of an observability matrix.  
         [0016]     Another aspect of the present invention is to provide a method of localizing a robot wherein a Kalman filter method for parameters having known constraints is applied so that the constraints are satisfied.  
         [0017]     Additional aspects and/or advantages of the invention will be set forth in part in the description which follows and, in part, will be apparent from the description, or may be learned by practice of the invention.  
         [0018]     The foregoing and/or other aspects are achieved by providing a robot including a gyroscope module providing information regarding a rotational angle of a gyroscope; an encoder module for providing information regarding velocity and information regarding rotational angle of a wheel of the robot by sensing motion of the wheel; and a control module estimating a current pose of the robot according to a method based on information provided by the encoder module and the gyroscope module, the control module incorporating information regarding the rotational angle provided by the gyroscope module when estimating the current pose.  
         [0019]     The foregoing and/or other aspects are also achieved by providing a method of localizing a robot having an encoder module and a gyroscope module, the method including: the gyroscope module providing information regarding a rotational angle of a gyroscope; the encoder module providing information regarding velocity and information regarding a rotational angle of a wheel, based on motion of the wheel; and localizing the robot based on the information provided by the encoder module and the gyroscope module by incorporating information regarding the rotational angle provided by the gyroscope module. 
     
    
     BRIEF DESCRIPTION OF THE DRAWINGS  
       [0020]     These and/or other aspects and advantages of the invention will become apparent and more readily appreciated from the following description of the embodiments, taken in conjunction with the accompanying drawings of which:  
         [0021]      FIG. 1  is a flowchart showing a method of localizing a robot according to an embodiment of the present invention;  
         [0022]      FIG. 2  shows an example of state variables and parameters of a mobile robot according to the embodiment of the present invention;  
         [0023]      FIG. 3  shows a state equation for the example of  FIG. 2  according to the related art;  
         [0024]      FIG. 4  shows a state equation for the example of  FIG. 2  according to the embodiment of the present invention;  
         [0025]      FIG. 5  shows a measurement equation for the example of  FIG. 2  according to the embodiment of the present invention;  
         [0026]      FIG. 6  shows a measurement equation for the example of  FIG. 2  according to the related art;  
         [0027]      FIG. 7  is a flowchart showing operation S 150  of  FIG. 1  in more detail according to the embodiment of the present invention;  
         [0028]      FIG. 8  is a block diagram showing the construction of a robot according to the embodiment of the present invention;  
         [0029]      FIG. 9A  is a graph showing an experimental result, when constraints are not considered, according to the related art;  
         [0030]      FIG. 9B  is a graph showing an experimental result, when constraints are considered, according to the embodiment of the present invention; and  
         [0031]      FIG. 10  is a graph showing comparison of mean processing time for 1 cycle of a Kalman filter according to the related art and the present embodiment.  
     
    
     DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT  
       [0032]     Reference will now be made in detail to the embodiment of the present invention, an example of which is illustrated in the accompanying drawings, wherein like reference numerals refer to the like elements throughout. The embodiment is described below to explain the present invention by referring to the figures.  
         [0033]     The present invention is described hereinafter with reference to flowchart illustrations of a robot and a method of localizing the robot according to the embodiment of the invention. It should be understood that each block of the flowchart illustrations, and combinations of blocks in the flowchart illustrations, can be implemented, for example, by computer program instructions. These computer program instructions can be provided to a processor of a general purpose computer, special purpose computer, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means of implementing the functions specified in the flowchart block or blocks.  
         [0034]     These computer program instructions may also be stored in a computer usable or computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer usable or computer-readable memory produce an article of manufacture including instruction means that implement the function specified in the flowchart block or blocks.  
         [0035]     The computer program instructions may also be loaded into a computer or other programmable data processing apparatus to cause a series of operations to be performed in the computer or other programmable apparatus to produce a computer implemented process such that the instructions that execute on the computer or other programmable apparatus provide operations to implement the functions specified in the flowchart block or blocks.  
         [0036]     Each block of the flowchart illustrations may represent a module, segment, or portion of code, which includes one or more executable instructions to implement the specified logical function(s). It should also be noted that in some alternative implementations, the functions noted in the blocks may occur out of order. For example, two blocks shown in succession may in fact be executed substantially concurrently or the blocks may sometimes be executed in reverse order, depending upon the functionality involved.  
         [0037]      FIG. 1  is a flowchart showing a method of localizing a robot according to an embodiment of the present invention.  
         [0038]     Referring to  FIG. 1 , a state variable is initially predicted S 110 . A new state variable is added to a conventional state equation so that the size of the rank of an observability matrix is improved. This stabilizes the state of the robot localization process.  
         [0039]      FIG. 2  shows state variables and parameters of a mobile robot.  
         [0040]     The observability matrix for dynamic system model over a discrete-time interval can be defined by equation 1:  
               O   ⁡     (       H   k     ,     F   k     ,     1   ≤   k   ≤     k   f         )       =       ∑     k   =   1       k   f       ⁢         [       ∏     i   =   0       k   -   1       ⁢           ⁢     F     k   -   i         ]     T     ⁢     H   k   T     ⁢       H   k     ⁡     [       ∏     i   =   0       k   -   1       ⁢           ⁢     F     k   -   i         ]                   (   1   )             
 
 where H k  refers to an observation matrix at time k, T is a transpose of the matrix, and F k  refers to a state transition matrix from k to (k+1) for 1≦k≦k f . 
 
         [0041]     When the state transition matrix of the dynamic system model is time-invariant, the observability can be characterized by a rank of the matrix defined by equation (2): 
 
 O=[H   T    F   T   H   T  ( F   T ) 2   H   T  . . . ( F   T ) n−1   H   T ]  (2) 
 
         [0042]     The system is observable if the rank of equation (2) is n, which corresponds to the dimension of the system state vector.  
         [0043]     As mentioned above, according to the related art, the rank is 2, which means that the robot system is not fully observable.  
         [0044]     The embodiment of the present invention uses a gyroscope and encoders for both wheels so that a pose of the robot, in which rotational angle of the gyroscope is incorporated, is observable.  
         [0045]     The position and heading angle φ(k) of the robot can be defined by equation (3):  
                           X   ^     G     ⁡     (     k   +   1     )       =       ⁢           X   ^     G     ⁡     (   k   )       +     cos   ⁢       ϕ   ^     ⁡     (   k   )       ×                       ⁢     (           (     1   +       S   R     ⁡     (   k   )         )     ⁢       U   R     ⁡     (   k   )         +       (     1   +       S   L     ⁡     (   k   )         )     ⁢       U   L     ⁡     (   k   )           2     )             ⁢           ⁢                 Y   ^     G     ⁡     (     k   +   1     )       =       ⁢           Y   ^     G     ⁡     (   k   )       +     sin   ⁢       ϕ   ^     ⁡     (   k   )       ×                       ⁢     (           (     1   +       S   R     ⁡     (   k   )         )     ⁢       U   R     ⁡     (   k   )         +       (     1   +       S   L     ⁡     (   k   )         )     ⁢       U   L     ⁡     (   k   )           2     )             ⁢     
     ⁢           X   ^     G     ⁡     (   k   )       =       X   ⁡     (   k   )       +     δ   ⁢           ⁢       X   G     ⁡     (   k   )             ⁢     
     ⁢           Y   ^     G     ⁡     (   k   )       =       Y   ⁡     (   k   )       +     δ   ⁢           ⁢       Y   G     ⁡     (   k   )                     (   3   )             
 
 where {circumflex over (X)} G (k) and Ŷ G (k) refer to the calculated position using calculated gyro angle, δX G (k) and δYG ( k) refer to errors in position of the robot estimated by the gyroscope, and φ refers to the rotational angle of the gyroscope. 
 
         [0046]     By adding δX G (k) and δY G (k) to the state equation shown in  FIG. 3  as new state variables, the state equation is modified as shown in  FIG. 4 , wherein reference numeral  410  indicates newly added state variables, and  420  indicates a newly added portion of the system matrix.  
         [0047]     The state equation may be considered as a process model which models a change of the state variable according to the elapse of time.  
         [0048]     State estimation based on  FIG. 4  is defined by equation (4): 
 
 {circumflex over (x)}   k   =f ( x   k−1   , i   k , 0) 
 
 P   k   −   =F   k   P   k−1   F   k   T   +Q   k    (4) 
 
 where x refers to a state variable in  FIG. 4 ; u refers to a velocity component of the robot; F refers to matrix F in  FIG. 4 ; P refers to a covariance matrix; Q refers to a variance regarding noise; and F k =∂f/∂x(x k , u k , 0) 
 
         [0049]     Based on a state equation having estimated state variables, as shown in  FIG. 4 , the state is observed S 120 .  
         [0050]     An observation model such as a measurement equation may be used to this end. A measurement equation according to the embodiment of the present invention is defined by equation 5 below and is illustrated in  FIG. 5 .  
         [0051]     The observation model represents a modeling for a combination of observable state variables 
 
equation=pose of robot estimated by encoder−pose of robot estimated by gyroscope+noise   (5) 
 
 In  FIG. 5 , “v” refers to measurement noise. 
 
         [0052]     In contrast to the related art, the observation matrix H of the measurement equation according to the present embodiment, shown in  FIG. 5 , includes not only an angular state, but also a state related to the position of the robot. As a result, the rank of the observability matrix, as defined by equation (1), is improved.  
         [0053]     Particularly, the present rank(O)=4, in the case of a time-invariant system. This satisfies a condition that error in x, y distances converge to a true value. In the case of a time-varying system, rank(O)=10, thereby stabilizing the system. It is to be noted that state variable δB s (k+1) of  FIG. 4  corresponds to error in scale factor of the gyroscope. The variable δB s (k+1) is related to the calibration of the gyroscope prior to calculation of rank and is excluded when calculating the rank.  
         [0054]     After observing the state of the robot in operation  120  and obtaining Z k  including observation matrix H (in this case, H and Z k  refer to H and Z shown in  FIG. 6 , respectively), Kalman gain is obtained based on the H. The Kalman gain and Z k  are used to update the state variable x and covariance P in equation (4), as defined by equation 7: 
 
 K   k   =P   k   −   H   k   T ( H   k   P   k   −   H   k   T   +R   k ) −1  
 
 {circumflex over (x)}   k   ={circumflex over (x)}   k   −   +K   k ( z   k   −h ( {circumflex over (x)}   k   − , 0)) 
 
 P   k ( I−K   k   H   k ) P   k   −   (7) 
 
 Wherein, K k  refers to Kalman gain, I is an identity matrix, R k  refers to variance regarding noise, and H k =∂h/∂x(x k , u k , 0) 
 
         [0055]     When the mobile robot has no constraints (S 140 ), the state variable {circumflex over (x)} k , which was updated in operation S 130 , is used to localize the mobile robot, and operation S 110  is repeated.  
         [0056]     When the mobile robot has constraints (S 140 ), the state variable {circumflex over (x)} k , which has been updated in operation S 130 , is amended in conformity with the constraints so that the mobile robot is localized based on the amended state variables (S 150 ).  
         [0057]     The constraints of the mobile robot, which has a gyroscope loaded thereon, include wheel tread distance, wheel diameter, as well as scale factor and bias of the gyroscope resulting from temperature and angle rate change. In addition, the constraints may include linear constraints, non-linear constraints, inequality constraints, and equality constraints.  
         [0058]      FIG. 7  is a flowchart showing operation S 150  of  FIG. 1  in more detail. Operation S 150  includes operation S 152  of updating state variable {circumflex over (x)} k  by using constraints and operation S 154  of updating a penalty parameter and a Lagrange multiplier.  
         [0059]     Those skilled in the art can understand that the present invention is directed to a Kalman filter problem with constraints. In order to solve this problem, a state variable without constraints is projected onto a constraint plane so that a solution is obtained.  
         [0060]     Particularly, with constraints g and h, a solution is obtained from equation (8) below, under a condition of g i (x)≦0 and h j (x)=0.  
                 min   x     ⁢     f   ⁡     (   x   )         =         (     x   -     x   ^       )     T     ⁢     W   ⁡     (     x   -     x   ^       )                 (   8   )             
 
         [0061]     Here, g i (x) refers to inequality constraints; h j (x) refers to equality constraints; {circumflex over (x)} k  refers to a Kalman-filtered state; and W refers to any positive definite matrix, such as identity matrix or process noise covariance matrix.  
         [0062]     In the case of a constrained optimization problem, such as equation 8, the necessary condition for the existence of a state, which locally minimizes the constrained optimization problem to be minimized, is a Lagrange multiplier. Conventional Kuhn-Tucker optimality conditions can provide such a condition, for example.  
         [0063]     Equation (8) corresponds to a convex problem, and conventional penalty function theorem can be applied to equation 8.  
         [0064]     The penalty function theorem states that, when penalty parameter s k  gradually increases in a convex problem, the solution of a Lagrangian function L( ) converges to an optimal solution. The resulting penalty function is defined by equation 9: 
 
 L ( s   k   ,x )= f ( x )+ s   k /2[Σ i ( g   i   + ( x )) 2 +Σ j ( h   j ( x )) 2 ]  (9) 
 
 where g i   + (x)=max(0,g i (x)). 
 
         [0065]     By applying the penalty function theorem to equation 8, a solution of a state variable, which has been updated by using constraints, is defined by equation 10: 
 
 {dot over (x)}   k =−2 W   T ( x   k   −{circumflex over (x)}   k )−[Σ i   ∇g   i ( s   k   g   i   + +λ i )+Σ j   ∇h   j ( s   k   h   j +μ j )]  (10) 
 
 Wherein, {dot over (λ)} i =εs k g i   + , {dot over (μ)} j =εs k h j , s k =(1+α)s k−1 , α≧0, ε≧0. 
 
         [0066]     In addition, {circumflex over (x)} k  refers to a state variable updated in operation S 130 ; x k  refers to a state variable updated by using constraints; g refers to inequality constraints; h refers to equality constraints; s refers to a penalty parameter; and λ and μ refer to Lagrange multipliers.  
         [0067]     The penalty parameter s can be selected based on the above-mentioned penalty function theorem.  
         [0068]     A process for updating the penalty parameter s and Lagrange multipliers λ and μ will now be described.  
         [0069]     Initially, ε=0, α=positive constant, and, after a predetermined time, ε is also modified into a small positive constant, so that s, λ, μ are updated. If necessary, α may be modified into another positive constant after a predetermined time, in order to improve the convergence rate.  
         [0070]     As defined by equation 10, update of a constrained Kalman filter satisfies both Kuhn-Tucker optimality conditions and penalty function theorem. Thus, the update of the constrained Kalman filter is regarded as an optimal solution.  
         [0071]     Particularly, according to the related art, only linear constraints are considered and, in order to obtain a solution, the inverse function of an arbitrary positive definite matrix W must be calculated. In contrast, the embodiment of the present invention can be applied even when the constraints are arbitrary non-linear functions, and no calculation of the inverse function of W is necessary, as defined by equation 10. As such, the embodiment of the present invention provides a substantially improved algorithm over the related art.  
         [0072]      FIG. 8  is a block diagram showing the construction of a robot according to an embodiment of the present invention. The robot 800 includes a gyroscope module  810 , an encoder module  820 , a driving module  830 , and a control module  840 .  
         [0073]     The term ‘module’, as used herein, means, but is not limited to, a software or hardware component, such as a Field Programmable Gate Array (FPGA) or an Application Specific Integrated Circuit (ASIC), which performs certain tasks. A module may advantageously be configured to reside on the addressable storage medium and configured to execute on one or more processors. Thus, a module may include, by way of example, components, such as software components, object-oriented software components, class components and task components, processes, functions, attributes, procedures, subroutines, segments of program code, drivers, firmware, microcode, circuitry, data, databases, data structures, tables, arrays, and variables. The functionality provided for in the components and modules may be combined into fewer components and modules or further separated into additional components and modules.  
         [0074]     In addition, “robot”, as used herein, refers to an object adapted to localize itself by using a gyroscope and encoders.  
         [0075]     The gyroscope module  810  is attached to the mobile robot  800  and provides information regarding the rotational angle of the gyroscope, as labeled φ in  FIG. 4 .  
         [0076]     The encoder module  820  senses the operation of the driving module  830 , which includes wheels, and provides information regarding the velocity (U L , U R  in  FIG. 4 ) of the mobile robot  800  and the rotational angle (ψ) in  FIG. 4 ) of the wheels.  
         [0077]     The control module  840  localizes the mobile robot  800  based on the information inputted from the encoder module  820  and the gyroscope module 810 by using the above-mentioned Kalman filter method with constraints. By considering the pose information of the mobile robot  800 , in which the φ information is incorporated, the stability of the mobile robot  800  system is improved.  
         [0078]     After localizing the mobile robot  800 , the control module  840  operates the driving module  830  accordingly so that the mobile robot  800  is moved.  
         [0079]     For comparison, experiments have been performed with robot parameters, including a wheel tread diameter D of 0.378 m and a wheel diameter R of 0.122 m, as well as gyroscope parameters, including gyroscope scale factor (rate to angle scale factor) B s  of 0.01, and gyroscope rate bias B b  of 0 (deg/sec).  
         [0080]     An example of parameter constraints used in the experiments is defined below. 
 
| S   R |≦0.1 
 
| S   L |≦0.1 
 
|δ D|≦ 0.005 
 
|δ B   rb |≦0.01 
 
         [0081]      FIG. 9A  is a graph showing an experiment result, when constraints are not considered, according to the related art, and  FIG. 9B  is a graph showing an experiment result, when constraints are considered, according to the embodiment of the present invention.  
         [0082]     It is clear from comparison between  FIGS. 9A and 9B  that constraints regarding δD are not satisfied in  FIG. 9A , but are satisfied in  FIG. 9B .  
         [0083]      FIG. 10  is a graph showing comparison of mean processing time for 1 cycle of a Kalman filter.  
         [0084]     Referring to  FIG. 10 , bars labeled EKF show mean processing time according to the related art, bars labeled C-KF show mean processing time according to the embodiment of the present invention, bars labeled UKF show mean processing time based on Unscented Kalman Filter method, and bars labeled SR-UKF show mean processing time based on Square-Root Unscented Kalman Filter method.  
         [0085]     It is clear that there is little difference between the present mean processing time and that of the related art. Thus, even though the embodiment of the present invention reduces error in locating a robot while satisfying physical constraints, the processing time is not increased.  
         [0086]     Although a Kalman filter with constraints is used to locate a robot in the embodiment of the present invention, it is only an example and does limit the present invention in any manner.  
         [0087]     As mentioned above, the present invention can improve the rank of an observability matrix and guarantee the stability of a robot. In addition, a Kalman filter is used so that conventional non-linear constraints are satisfied. This improves the system stability. Furthermore, K-T optimality conditions are satisfied in the case of update for satisfying constraints. This design provides an optimal solution in the current state.  
         [0088]     Although an embodiment of the present invention has been shown and described, it would be appreciated by those skilled in the art that changes may be made in this embodiment without departing from the principles and spirit of the invention, the scope of which is defined in the claims and their equivalents.