Robot and method of localizing the same

• US 20070106423A1
• Filed: 11/07/2006
• Published: 05/10/2007
• Est. Priority Date: 11/07/2005
• Status: Active Grant

• Current Assignee: Samsung Electronics Co. Ltd.
• Original Assignee: Samsung Electronics Co. Ltd.
• Inventors: Choi, Ki-wan, Myeong, Hyeon, Lee, Hyoung-ki, Lee, Yong-beom
• Granted Patent: US 7,826,926 B2
• Time in Patent Office: Days
• US Class Current: 700/245
• CPC Class Codes: G05D 1/027 comprising intertial navigation means, e.g. azimuth detector inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00

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.

Claims

1. A robot comprising:

a gyroscope module providing information regarding a rotational angle of a gyroscope;

an encoder module 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 the information regarding the rotational angle provided by the gyroscope module when estimating the current pose.

2. The robot of claim 1, wherein the method comprises a Kalman filter method having constraints.

3. The robot of claim 2, wherein the Kalman filter method satisfies Kuhn-Tucker optimality conditions and penalty function theorem.

4. The robot of claim 2, wherein the constraints are expressed as penalty functions.

5. The robot of claim 1, wherein the control module is adapted to localize the robot by using the estimated pose of the robot and a pose error of the robot estimated by the gyroscope module as state variables.

6. A method of localizing a robot having an encoder module and a gyroscope module, comprising:

providing information regarding a rotational angle of a gyroscope from the gyroscope module;

providing information regarding velocity and information regarding a rotational angle of a wheel of the robot, based on motion of the wheel, from the encoder module; and

localizing the robot based on the information provided from the encoder module and the gyroscope module by incorporating information regarding the rotational angle provided by the gyroscope module.

7. The method of claim 6, wherein the localizing the robot comprises:

predicting a state variable by a process model of localizing the robot;

observing an observation model by a measurement equation; and

updating the predicted state variable.

8. The method of claim 7, wherein the process model is expressed by a state equation:

$\left[\begin{array}{c}\mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>X<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>Y<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>X_G<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>Y_G<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k+1\right)\\ S_R<br><br>\left(k+1\right)\\ S_L<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>D<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>\varphi <br><br>}<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>B_S<br><br>\left(k+1\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>B_{\mathrm{rb}}<br><br>\left(k+1\right)\end{array}\right]=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& -\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R+U_L}{2}& \cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R}{2}& \cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_L}{2}& 0& 0& 0& 0\\ 0& 1& 0& 0& \cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R+U_L}{2}& \sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R}{2}& \sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_L}{2}& 0& 0& 0& 0\\ 0& 0& 1& 0& -\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\varphi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R+U_L}{2}& \cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\varphi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R}{2}& \cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\varphi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_L}{2}& 0& 0& 0& 0\\ 0& 0& 0& 1& \cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\varphi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R+U_L}{2}& \sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\varphi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_R}{2}& \sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\varphi <br><br>}<br><br>\left(k\right)<br><br>\frac{U_L}{2}& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& \frac{U_R}{D}& \frac{U_L}{D}& \frac{U_L-U_R}{D^2}& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& \mathrm{\Omega <br><br>}& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]<br><br>\left[\begin{array}{c}\mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>X<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>Y<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>X_G<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>Y_G<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)\\ S_R<br><br>\left(k\right)\\ S_L<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>D<br><br>\left(k\right)\\ \mathrm{\delta <br><br>\varphi <br><br>}<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>B_S<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>B_{\mathrm{rb}}<br><br>\left(k\right)\end{array}\right]+w,$

where δ

X_G(k) and δ

Y_G(k) indicate position error of the robot estimated by the gyroscope module, and φ

indicates a rotational angle provided by the gyroscope module, S_R(k) and S_L(k) refer to scale factor errors of the encoder module, D(k) refers to a tread error, δ

B_s is an error in scale factor of the gyroscope, U is a velocity of the wheel, w is process noise and ψ

is a rotational angle of the wheel.

9. The method of claim 7, wherein the measurement equation=the estimated pose of the robot−

the pose of the robot estimated by the gyroscope+noise.

10. The method of claim 9, wherein the measurement equation is defined by:

$\begin{array}{c}Z=<br><br>\left[\begin{array}{c}\hat{X}<br><br>\left(k\right)-{\hat{X}}_G<br><br>\left(k\right)\\ \hat{Y}<br><br>\left(k\right)-{\hat{Y}}_G<br><br>\left(k\right)\\ \widehat{\mathrm{\psi <br><br>}}<br><br>\left(k\right)-\widehat{\mathrm{\varphi <br><br>}}<br><br>\left(k\right)\end{array}\right]+v\\ =<br><br>\left[\begin{array}{c}\mathrm{\delta <br><br>}<br><br>\hat{X}<br><br>\left(k\right)-{\widehat{\mathrm{\delta <br><br>X}}}_G<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\hat{Y}<br><br>\left(k\right)-\mathrm{\delta <br><br>}<br><br>{\hat{Y}}_G<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\widehat{\mathrm{\psi <br><br>}}<br><br>\left(k\right)-\mathrm{\delta <br><br>}<br><br>\widehat{\mathrm{\varphi <br><br>}}<br><br>\left(k\right)\end{array}\right]+v\\ =<br><br>\left[\begin{array}{ccccccccccc}1& 0& -1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& -1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& -1& 0& 0\end{array}\right]<br><br>\left[\begin{array}{c}\mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>X<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>Y<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>X_G<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>Y_G<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\psi <br><br>}<br><br>\left(k\right)\\ S_R<br><br>\left(k\right)\\ S_L<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>D<br><br>\left(k\right)\\ \mathrm{\delta <br><br>\varphi <br><br>}<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>B_S<br><br>\left(k\right)\\ \mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>B_{\mathrm{rb}}<br><br>\left(k\right)\end{array}\right]+v\end{array}$

wherein {circumflex over (X)}_G(k) and Ŷ

_G(k) indicate a position of the robot estimated by using rotational angle provided by the gyroscope module and v is measurement noise.

11. The method of claim 7, wherein, the updating comprises updating the predicted state variable by

K_k=P_k⁻

H_k^T(H_kP_k⁻

H_k^T+R_k)<sup>−

1</sup>
{circumflex over (x)}_k={circumflex over (x)}_k⁻

+K_k(z_k−

h({circumflex over (x)}_k⁻

,0))
P_k=(I−

K_kH_k)P_k⁻

where K_k is Kalman gain, H_k is an observation matrix, T is a transpose of the matrix, I is an identity matrix, P_k is a covariance matrix, R_k is variance regarding noise, {circumflex over (x)}_k⁻

is a state variable before update, {circumflex over (x)}_k is a state variable after update, z_k is a measurement equation, h refers to inequality constraints, and H_k=∂

h/∂

x(x_k,u_k,0).

12. The method of claim 6, wherein the localizing comprises localizing the robot according to a Kalman filter method having constraints.

13. The method of claim 12, wherein the Kalman filter method satisfies Kuhn-Tucker optimality conditions and penalty function theorem.

14. The method of claim 12, wherein the constraints are expressed as penalty functions.

15. The method of claim 12, wherein, based on the Kalman filter method having constraints, a state equation of the robot is defined by:

{dot over (x)}_k=−

2L^T(x_k−

{circumflex over (x)}_k)−

[Σ

_i∇

g_i(s_kg_i⁺+λ

_i)+Σ

_j∇

h_j(s_kh_j+μ

_j)]

where x_k is a state variable for localizing the robot, T is a transpose of the matrix, W is a positive definite matrix, {circumflex over (x)} is a Kalman-filtered state variable, g_i(x) indicates inequality constraints, h_j(x) indicates equality constraints, “

s”

is a penalty parameter, λ and

μ

are Lagrange multipliers, and {dot over (λ

)}_iε

s_kg_i⁺, {dot over (μ

)}_j=ε

s_kh_j, s_k=(1+α

)s<sub>k−

1</sub>, α

≧

0, ε

≧

0.

16. The method of claim 15, wherein initially ε

=0, α

=positive constant the method further comprising modifying ε

into a small positive constant after a predetermined time so that s, λ

, and μ

are updated.

17. The method of claim 16, the method further comprising modifying α

into a different positive constant after a predetermined time.

18. The method of claim 8, wherein a rank of an observability matrix is 4 for a time invariant system.