Localization system and method of mobile robot based on camera and landmarks

• US 7,634,336 B2
• Filed: 08/23/2006
• Issued: 12/15/2009
• Est. Priority Date: 12/08/2005
• Status: Expired due to Fees

• Current Assignee: Electronics and Telecommunications Research Institute
• Original Assignee: Electronics and Telecommunications Research Institute
• Inventors: Cho, Young Jo, Yu, Won Pil, Lee, Jae Young, Chae, Heesung
• Primary Examiner(s): Black; Thomas G
• Assistant Examiner(s): Nolan; Peter D
• Application Number:
  US11/508,716
• Publication Number:
  US 20070150097A1
• Time in Patent Office: 1,210 Days
• Field of Search: 700/245, 700/258, 700/259, 701/23, 701/28, 702/150, 702/151, 180/167, 180/168
• US Class Current: 701/28
• CPC Class Codes: G05D 1/0234 using optical markers or beacons optical beacons per se G01S1/70
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30

Abstract

A localization system and method of a mobile robot using a camera and artificial landmarks in a home and a general office environment (or working zone) is provided. The localization system includes artificial landmarks having an LED flash function in an invisible wavelength band, a camera with a wide-angle lens, a module flashing landmarks attached at the ceiling and identifying positions and IDs of the landmarks from an image photographed by the camera having a filter, a module calculating position and orientation of the robot using two landmarks of the image in a stop state, a module, when a ceiling to which the landmarks are attached has different heights, a position of the robot, and a module, when a new landmark is attached in the working zone, calculating a position of the new landmark on an absolute coordinate.

Claims

1. A localization system of a mobile robot, comprising:

n number of landmarks attached at a ceiling and having wireless transmitting and receiving functions,

a camera photographing the n number of landmarks;

a landmark detection part which flashes the landmarks attached at the ceiling and obtains positions and IDs of the landmarks from an image photographed by the camera to detect at least two landmarks;

a robot position detection part calculating the position of the mobile robot using the detected landmarks, the robot position detection part comprising a first module calculating current position and orientation of the mobile robot using an image of the detected two landmarks when the mobile robot is in a stop state such that the first module calculates the current position ‘

Pw’

of the mobile robot using the equation P_w=R_we·

s(R_ie·

(P_i−

T_ie))+T_we, where

P_w is a real position of the mobile robot on a world coordinate that is the calculated final position of the mobile robot,

P_i is a position of the mobile robot on an image coordinate,

R_we is a rotational vector informing how much an extra coordinate rotationally moves with respect to the world coordinate,

R_ie is a rotational vector informing how much the image coordinate rotationally moves with respect to the extra coordinate,

T_ie is a parallel movement vector informing how much the image coordinate moves in parallel with respect to the extra coordinate,

T_we is a parallel movement vector informing how much the extra coordinate moves in parallel with respect to the world coordinate, and

s is a distance ratio between the two landmarks on the image and the real two landmarks;

a landmark position prediction part which, when a new landmark is attached within a working zone, calculates a position of the new landmark on an absolute coordinate;

a topology map building part which builds a topology map of the mobile robot using the calculated position of the new landmark; and

a robot controller controlling a navigation of the mobile robot using the built topology map.

2. The localization system of claim 1, wherein the n number of landmarks each have a specific ID built therein, comprises an infrared LED emitting light in a specific wavelength band, and wireless transmitting and receiving modules, and the camera comprises a camera filter which passes a light with the specific wavelength band to detect the infrared LED of the landmark.

3. The localization system of claim 2, wherein the LED of each of the n number of landmarks is flashed by each of the n number of landmarks and a wireless communication function of the camera.

4. The localization system of claim 1, wherein the landmark detection part detects at least two landmarks existing on an image of the camera by applying a binary search to the n number of landmarks attached at the ceiling.

5. The localization system of claim 1, wherein the robot position detection part further comprises:

a second module calculating a position of the mobile robot in real time when the mobile robot is in a moving state; and

a third module calculating a position of the mobile robot when the ceiling to which the n number of landmarks are attached has different heights.

6. The localization system of claim 5, wherein the third module calculates the position of the mobile robot by correcting the image coordinates of the n number of landmarks in accordance to

$\begin{array}{c}{P}^{\prime }\end{array}$

⁡

( u j ⁢

v j ) = P ·

h 1 h 2 ,

where P′

is the position of the mobile robot obtained from the corrected image coordinates of the n number of landmarks, P is the position of the mobile robot obtained from the n number of landmarks before being corrected, and h1 and h2 are heights of the ceiling.

7. The localization system of claim 5, wherein the second module calculates the position of the mobile robot by setting a mask with a constant size based on the image coordinates of the obtained two landmarks, and searching only an area of the mask set for the image coordinates of the landmarks after the moving of the mobile robot.

8. A localization system of a mobile robot, comprising:

n number of landmarks attached at a ceiling and having wireless transmitting and receiving functions,

a camera photographing the n number of landmarks;

a landmark detection part which flashes the landmarks attached at the ceiling and obtains positions and IDs of the landmarks from an image photographed by the camera to detect at least two landmarks;

a robot position detection part calculating the position of the mobile robot using the detected landmarks

a landmark position prediction part which, when a new landmark is attached within a working zone, calculates a position of the new landmark on an absolute coordinate;

wherein when a new landmark is attached in the working zone, the landmark position prediction part identifies an ID of the new landmark,

calculates a real position ‘

Pw’

of the mobile robot on a world coordinate that is the calculated final position of the mobile robot in accordance to

P_w=R_we·

s(R_ie·

(P_i−

T_ie))+T_we

where

P_i is a position of the mobile robot on an image coordinate,

R_we is a rotational vector informing how much an extra coordinate rotationally moves with respect to the world coordinate,

R_ie is a rotational vector informing how much the image coordinate rotationally moves with respect to the extra coordinate,

T_ie is a parallel movement vector informing how much the image coordinate moves in parallel with respect to the extra coordinate,

T_we is a parallel movement vector informing how much the extra coordinate moves in parallel with respect to the world coordinate,

s is a distance ratio between the two landmarks on the image and the real two landmarks, and

calculates a real position Gw of the new landmark in accordance to

G_w=R_we·

s(R_ie·

(G_i−

T_ie))+T_we where

Gi is a position of the new landmark on the image coordinate, and Gw is a real position of the new landmark;

a topology map building part which builds a topology map of the mobile

robot using the calculated position of the new landmark; and

a robot controller controlling a navigation of the mobile robot using the built topology map.

9. The localization system of claim 8, wherein the landmark detection part detects at least two landmarks existing on an image of the camera by applying a binary search to the n number of landmarks attached at the ceiling.

10. The localization system of claim 8, wherein the topology map building part builds the topology map by setting the landmarks as nodes and obtaining a distance information between the nodes.

11. The localization system of claim 8, wherein the n number of landmarks each have a specific ID built therein, comprises an infrared LED emitting light in a specific wavelength band, and wireless transmitting and receiving modules, and the camera comprises a camera filter which passes a light with the specific wavelength band to detect the infrared LED of the landmark.

12. The localization system of claim 11, wherein the LED of each of the n number of landmarks is flashed by each of the n number of landmarks and a wireless communication function of the camera.

13. The localization system of claim 8, wherein the robot position detection part comprises:

a first module calculating current position and orientation of the mobile robot using an image of the detected two landmarks when the mobile robot is in a stop state;

a second module calculating a position of the mobile robot in real time when the mobile robot is in a moving state; and

a third module calculating a position of the mobile robot when the ceiling to which the n number of landmarks are attached has different heights.

14. The localization system of claim 13, wherein the second module calculates the position of the mobile robot by setting a mask with a constant size based on the image coordinates of the obtained two landmarks, and searching only an area of the mask set for the image coordinates of the landmarks after the moving of the mobile robot.

15. The localization system of claim 13, wherein the third module calculates the position of the mobile robot by correcting the image coordinates of the n number of landmarks in accordance to

$\begin{array}{c}{P}^{\prime }\end{array}$

⁡

( u j ⁢

v j ) = P ·

h 1 h 2 ,

where

P′

is the position of the mobile robot obtained from the corrected image coordinates of the n number of landmarks,

P is the position of the mobile robot obtained from the n number of landmarks before being corrected, and

h1 and h2 are heights of the ceiling.

16. A localization method of a mobile robot using a camera and a landmark, the localization method comprising the steps of

detecting the landmark from an image of the landmark photographed by the camera to calculate a position of a mobile robot, such that the detecting step comprises the steps of:

acquiring an ID of the landmark using a wireless transmitting and receiving module of the mobile robot, flashing the landmark and photographing an image of the landmark of a ceiling;

performing a binary search for the photographed image of the landmark to detect at least two landmarks; and

calculating position and orientation of the mobile robot using the detected at least two landmarks wherein

when the mobile robot is in a stop state, calculating a final position ‘

P_w’

of the mobile robot using the detected at least two landmarks in accordance to

P_w=R_we·

s(R_ie·

(P_i−

T_ie))+T_we,

when the mobile robot is in a moving state, calculating the position of the mobile robot by setting a mask with a predetermined size based on image coordinates of the at least two landmarks and searching an area of the mask set for the image coordinates of the landmarks after a moving of the mobile robot, wherein

P_w is a real position of the mobile robot on a world coordinate that is the calculated final position of the mobile robot,

P_i is a position of the mobile robot on an image coordinate,

R_we is a rotational vector informing how much an extra coordinate rotationally moves with respect to the world coordinate,

s is a distance ratio between two landmarks on the image coordinate and on the world coordinate

R_ie is a rotational vector informing how much the image coordinate rotationally moves with respect to the extra coordinate,

T_ie is a parallel movement vector informing how much the image coordinate moves in parallel with respect to the extra coordinate,

T_we is a parallel movement vector informing how much the extra coordinate moves in parallel with respect to the world coordinate, and

s is a distance ratio between the two landmarks on the image and the real two landmarks;

obtaining a position of the added landmark using the calculated position of the mobile robot when a new landmark is added to a working zone;

setting the added landmark as a node to build a topology map; and

controlling a navigation of the mobile robot using the built topology map and the calculated position of the mobile robot.

17. The localization method of claim 16, wherein in the acquiring step, the landmark comprises an infrared LED and the camera comprises an infrared filter which passes only a specific wavelength band, and the mobile robot flashes the infrared LED and then photographs the landmark.

18. The localization method of claim 16, wherein the step of calculating position and orientation of the mobile robot comprises, calculating the position of the mobile robot by correcting the image coordinates of the landmarks in accordance to

$\begin{array}{c}{P}^{\prime }\end{array}$

⁡

( u j ⁢

v j ) = P ·

h 1 h 2 ,

when the ceiling to which the landmarks are attached has different heights, where

P′

is the position of the mobile robot obtained from the corrected image coordinates of the n number of landmarks,

P is the position of the mobile robot obtained from the n number of landmarks before being corrected, and

h1 and h2 are heights of the ceiling.

19. The localization method of claim 16, wherein the topography map is built by calculating a real position Gw of the new landmark in accordance to G_w=R_we·

s(R_ie(G_i−

T_ie))+T_we where Gi is a position of the new landmark on the image coordinate.