Method for self-localization of robot based on object recognition and environment information around recognized object

• US 20090210092A1
• Filed: 11/25/2008
• Published: 08/20/2009
• Est. Priority Date: 02/15/2008
• Status: Active Grant

• Current Assignee: Korea Advanced Institute of Science and Technology
• Original Assignee: Korea Advanced Institute of Science and Technology
• Inventors: Park, Sung Kee, Park, Soon Yong
• Granted Patent: US 8,024,072 B2
• Time in Patent Office: Days
• US Class Current: 700/259
• CPC Class Codes: B25J 19/023 including video camera means
• CPC Class Codes: B25J 5/00 Manipulators mounted on wheels or on carriages B25J1/00 takes precedence; programme-controlled manipulators B25J9/00 ; vehicle aspects B60, B62, e.g. remote-controlled steering for motor vehicles B62D1/24; control of position of vehicles G05D1/00
• CPC Class Codes: G05D 1/0251 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision stereoscopic image analysis H04N13/00; depth recovery from images G06T7/593
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06T 2207/10021 Stereoscopic video; Stereoscopic image sequence
• CPC Class Codes: G06T 2207/30252 Vehicle exterior; Vicinity of vehicle
• CPC Class Codes: G06T 7/80 Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration

Abstract

A method for self-localization of a robot, the robot including a camera unit, a database storing a map around a robot traveling path, and a position arithmetic unit estimating the position of the robot, includes: acquiring an image around the robot, in the camera unit. Further, the method includes recognizing, in the position arithmetic unit, an individual object in the image acquired by the camera unit, to generate position values on a camera coordinate system of local feature points of the individual objects and local feature points of a surrounding environment including the individual objects; and estimating, in the position arithmetic unit, the position of the robot on the basis of the map and the position values on the camera coordinate system of local feature points of the individual objects and local feature points of a surrounding environment including the individual objects.

Claims

1. A method for self-localization of a robot, the robot including a camera unit formed of a stereo video device, a database storing a map around a robot traveling path, and a position arithmetic unit estimating the position of the robot on the basis of image information acquired by the camera unit, the method comprising:

acquiring an image around the robot in the camera unit;

recognizing, in the position arithmetic unit, an individual object in the image acquired by the camera unit, to generate position values on a camera coordinate system of local feature points of the individual objects and local feature points of a surrounding environment including the individual objects; and

estimating, in the position arithmetic unit, the position of the robot on the basis of the map stored in the database and the position values on the camera coordinate system of local feature points of the individual objects and local feature points of a surrounding environment including the individual objects.

2. The method of claim 1, wherein estimating the position of the robot includes:

selecting nodes, on which at least recognized object is included, from the map stored in the database as candidate nodes,

wherein the robot can be located on the candidate node;

calculating temporary positions of the robot for all the candidate nodes on the basis of the position values; and

applying particle filtering to each candidate node with the calculated temporary position as an initial position value to estimate the position of the robot.

3. The method of claim 2, wherein the temporary positions of the robot are calculated by Equations 1 and 2,

$\begin{array}{c}\begin{array}{c}{\hspace{0.17em}}_{}^{}\end{array}\end{array}$

R N 

T = [ cos 

( θ

z ) 

cos 

( θ

y ) cos 

( θ

z ) 

sin 

( θ

y ) 

sin 

( θ

x ) - sin 

( θ

z ) 

cos 

( θ

x ) cos 

( θ

z ) 

sin 

( θ

y ) 

cos 

( θ

x ) + sin 

( θ

z ) 

sin 

( θ

x ) t x sin 

( θ

z ) 

cos 

( θ

y ) sin 

( θ

z ) 

sin 

( θ

y ) 

sin 

( θ

z ) + cos 

( θ

z ) 

cos 

( θ

x ) sin 

( θ

z ) 

sin 

( θ

y ) 

cos 

( θ

x ) - cos 

( θ

z ) 

sin 

( θ

x ) t y - sin 

( θ

y ) cos 

( θ

z ) 

sin 

( θ

x ) cos 

( θ

z ) 

cos 

( θ

x ) t z 0 0 0 1 ] = [ r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 ] [ Equation 



1 ]

^NV_i=^N_RT^RP_i 

 

[Equation 2]

where, ^RP_i and ^NV_i are a position value on the camera coordinate system and a position value on an environment map-based coordinate system of local feature points of the same object and the environment around the same object, respectively, ^N_RT is a homogenous transformation matrix of the two position values, and the position of the robot can be expressed by (x,y,z,θ

_x,θ

_y,θ

_z), and

wherein the position estimation step comprises;

calculating a final weighted value to apply the final weighted value to particle filtering; and

estimating the position of the robot by executing Equation 8 on the basis of only samples of a node, on which samples are most converged,

$\begin{array}{cc}\left[s\right]=\underset{n=1}{\overset{n_s}{\sum <br><br>}}<br><br>{\mathrm{\pi <br><br>}}_t^{\left(n\right)}<br><br>S_t^{\left(n\right)}& \left[\mathrm{Equation}<br><br><br><br>8\right]\end{array}$

where, [S] is the estimated position of the robot, s_t⁽ⁿ⁾ is a sample of the robot position to be predicted, π

_t⁽ⁿ⁾ is a weighted value of a sample s_t⁽ⁿ⁾ in a repetition step t, and .n_s is the total number of random samples.

4. The method of claim 1, wherein estimating the position of the robot includes:

selecting nodes, on which at least recognized object is included, from the map stored in the database as candidate nodes,

wherein the robot can be located on the candidate node;

calculating temporary positions of the robot for all the candidate nodes on the basis of the position values;

selecting a final candidate node from among the candidate nodes; and

applying particle filtering to the final candidate node with the temporary position of the robot for the final candidate node as an initial position value to estimate the position of the robot.

5. The method of claim 4, wherein the temporary positions of the robot are calculated by Equations 1 and 2,

$\begin{array}{c}\begin{array}{c}{\hspace{0.17em}}_{}^{}\end{array}\end{array}$

R N 

T = [ cos 

( θ

z ) 

cos 

( θ

y ) cos 

( θ

z ) 

sin 

( θ

y ) 

sin 

( θ

x ) - sin 

( θ

z ) 

cos 

( θ

x ) cos 

( θ

z ) 

sin 

( θ

y ) 

cos 

( θ

x ) + sin 

( θ

z ) 

sin 

( θ

x ) t x sin 

( θ

z ) 

cos 

( θ

y ) sin 

( θ

z ) 

sin 

( θ

y ) 

sin 

( θ

z ) + cos 

( θ

z ) 

cos 

( θ

x ) sin 

( θ

z ) 

sin 

( θ

y ) 

cos 

( θ

x ) - cos 

( θ

z ) 

sin 

( θ

x ) t y - sin 

( θ

y ) cos 

( θ

z ) 

sin 

( θ

x ) cos 

( θ

z ) 

cos 

( θ

x ) t z 0 0 0 1 ] = [ r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 ] [ Equation 



1 ]

^NV_I=^N_RT^RP_i 

 

[Equation 2]

where, ^RP_i and ^NV_i are a position value on the camera coordinate system and a position value on an environment map-based coordinate system of local feature points of the same object and the environment around the same object, respectively, ^N_RT is a homogenous transformation matrix of the two position values, and the position of the robot can be expressed by (x,y,z,θ

_x,θ

_y,θ

_z), and

wherein the position estimation step comprises;

calculating a final weighted value to apply the final weighted value to particle filtering; and

estimating the position of the robot by executing Equation 8,

$\begin{array}{cc}\left[s\right]=\underset{n=1}{\overset{n_s}{\sum <br><br>}}<br><br>{\mathrm{\pi <br><br>}}_t^{\left(n\right)}<br><br>S_t^{\left(n\right)}& \left[\mathrm{Equation}<br><br><br><br>8\right]\end{array}$

where, [S] is the estimated position of the robot, s_t⁽ⁿ⁾ is a sample of the robot position to be predicted, π

_t⁽ⁿ⁾ is a weighted value of a sample s_t⁽ⁿ⁾ in a repetition step t, and n_s is the total number of random samples.

6. The method of claim 4, wherein the final candidate node is a node, which has a maximum node proximity weighted value from among node proximity weighted values calculated by Equation 12 for the candidate nodes,

$\begin{array}{c}\begin{array}{c}{\pi }_{}^{}\end{array}\end{array}$

V ( N k ) = π

E ( N k ) + π

F ( N k ) + π

D ( N k ) ∑

k = 1 n N 

( π

E ( N k ) + π

F ( N k ) + π

D ( N k ) ) = 1 3 

( π

E ( N k ) + π

F ( N k ) + π

D ( N k ) ) [ Equation 



12 ]

where, π

_V^(Nk) is a node proximity weighted value of the robot for a node N_k, π

_E^(Nk) is an object existence weighted value of the node N_k, π

_F^(Nk) is an object-environment feature weighted value of the node N_k, π

_D^(Nk) is an object-environment distance weighted value of the node N_k, and n_N is the number of candidate nodes.

7. The method of claim 3, wherein the final weighted value is calculated by Equation 13 on the basis of the node proximity weighted value π

_V^(Nk) indicating a node, around which the robot is located, and a three-dimensional feature point weighted value π

_P⁽ⁿ⁾ of the sample s_t⁽ⁿ⁾ according to a distance difference between the three-dimensional feature points of an object recognized by the robot and the environment around the object and the three-dimensional feature points of the same object on a node and the environment around the object,

$\begin{array}{cc}{\mathrm{\pi <br><br>}}^{\left(n\right)}=\frac{{\mathrm{\pi <br><br>}}_P^{\left(n\right)}\times <br><br>{\mathrm{\pi <br><br>}}_V^{\left(N_k\right)}}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>\underset{n=1}{\overset{n_s}{\sum <br><br>}}<br><br>\left({\mathrm{\pi <br><br>}}_P^{\left(n\right)}\times <br><br>{\mathrm{\pi <br><br>}}_V^{\left(N_k\right)}\right)}& \left[\mathrm{Equation}<br><br><br><br>13\right]\end{array}$

where, π

⁽ⁿ⁾ is a final weighted value, and n_N is the number of candidate nodes.

8. The method of claim 5, wherein the final weighted value is calculated by Equation 13 on the basis of the node proximity weighted value π

_V^(Nk) indicating a node, around which the robot is located, and a three-dimensional feature point weighted value π

_P⁽ⁿ⁾ of the sample s_t⁽ⁿ⁾ according to a distance difference between the three-dimensional feature points of an object recognized by the robot and the environment around the object and the three-dimensional feature points of the same object on a node and the environment around the object,

$\begin{array}{cc}{\mathrm{\pi <br><br>}}^{\left(n\right)}=\frac{{\mathrm{\pi <br><br>}}_P^{\left(n\right)}\times <br><br>{\mathrm{\pi <br><br>}}_V^{\left(N_k\right)}}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>\underset{n=1}{\overset{n_s}{\sum <br><br>}}<br><br>\left({\mathrm{\pi <br><br>}}_P^{\left(n\right)}\times <br><br>{\mathrm{\pi <br><br>}}_V^{\left(N_k\right)}\right)}& \left[\mathrm{Equation}<br><br><br><br>13\right]\end{array}$

where, π

⁽ⁿ⁾ is a final weighted value, and n_N is the number of candidate nodes.

9. The method of claim 7, wherein the node proximity weighted value π

_V^(Nk) is determined by Equation 12,

$\begin{array}{cc}\begin{array}{c}{\mathrm{\pi <br><br>}}_V^{\left(N_k\right)}=\frac{{\mathrm{\pi <br><br>}}_E^{\left(N_k\right)}+{\mathrm{\pi <br><br>}}_F^{\left(N_k\right)}+{\mathrm{\pi <br><br>}}_D^{\left(N_k\right)}}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>\left({\mathrm{\pi <br><br>}}_E^{\left(N_k\right)}+{\mathrm{\pi <br><br>}}_F^{\left(N_k\right)}+{\mathrm{\pi <br><br>}}_D^{\left(N_k\right)}\right)}\\ =\frac{1}{3}<br><br>\left({\mathrm{\pi <br><br>}}_E^{\left(N_k\right)}+{\mathrm{\pi <br><br>}}_F^{\left(N_k\right)}+{\mathrm{\pi <br><br>}}_D^{\left(N_k\right)}\right)\end{array}& \left[\mathrm{Equation}<br><br><br><br>12\right]\end{array}$

where, π

_V^(Nk) is a node proximity weighted value of the robot for a node N_k, π

_E^(Nk) is an object existence weighted value of the node N_k, π

_F^(Nk) is an object-environment feature weighted value of the node N_k, π

_D^(Nk) is an object-environment distance weighted value of the node N_k, and n_N is the number of candidate nodes.

10. The method of claim 9, wherein the object existence weighted value π

_E^(Nk) is calculated by Equation 9,

$\begin{array}{cc}{\mathrm{\pi <br><br>}}_E^{\left(N_k\right)}=\frac{n<br><br>\left(E^{\left(R\right)}\bigcap <br><br>E^{\left(N_k\right)}\right)}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>n<br><br>\left(E^{\left(R\right)}\bigcap <br><br>E^{\left(N_k\right)}\right)}=\frac{n_E^{\left(R\bigcap <br><br>N_k\right)}}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>n_E^{\left(R\bigcap <br><br>N_k\right)}}& \left[\mathrm{Equation}<br><br><br><br>9\right]\end{array}$

where, π

_E^(Nk) is an object existence weighted value of a node N_k, E^(R) is a collection of objects recognized by the robot, E^(Nk) is a collection of objects on the node N_k, and n_E<sup>(R∩

Nk)</sup> is the number of same objects from among the objects recognized by the robot and the objects on the node N_k.

11. The method of claim 9, wherein the object-environment feature weighted value π

_F^(Nk) is calculated by Equation 10,

$\begin{array}{cc}{\mathrm{\pi <br><br>}}_F^{\left(N_k\right)}=\frac{n(\left(F^{\left(R\right)}\bigcap <br><br>F^{\left(N_k\right)}\right)}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>n(\left(F^{\left(R\right)}\bigcap <br><br>F^{\left(N_k\right)}\right)}=\frac{n_F^{\left(R\bigcap <br><br>N_k\right)}}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>n_F^{\left(R\bigcap <br><br>N_k\right)}}& \left[\mathrm{Equation}<br><br><br><br>10\right]\end{array}$

where, π

_F^(Nk) is an object-environment feature weighted value of a node N_k, F^(R) is a collection of the local feature points of all objects recognized by the robot and the local feature points of the environment around the objects, F^(Nk) is a collection of the local feature points of objects on the node N_k and the local feature points of the environment around the objects, n_F<sup>(R∩

Nk)</sup> is the number of matched local feature points between the local feature points of all objects recognized by the robot and the environment around the objects and the local feature points of the same objects on the node N_k as the objects recognized by the robot and the environment around the objects.

12. The method of claim 9, wherein the object-environment distance weighted value π

_D^(Nk) is calculated by Equation 11,

$\begin{array}{cc}{\mathrm{\pi <br><br>}}_D^{\left(N_k\right)}=\frac{{\left(1+{\stackrel{\_}{d}}^{\left(N_k\right)}\right)}^{-1}}{\underset{k=1}{\overset{n_N}{\sum <br><br>}}<br><br>{\left(1+{\stackrel{\_}{d}}^{\left(N_k\right)}\right)}^{-1}}<br><br><br><br>{\stackrel{\_}{d}}^{\left(N_k\right)}=\frac{1}{n_F^{\left(N_K\bigcap <br><br>R\right)}}<br><br>\underset{f=1}{\overset{n_F^{\left(N_K\bigcap <br><br>R\right)}}{\sum <br><br>}}<br><br>d_f^{\left(N_K\right)}& \left[\mathrm{Equation}<br><br><br><br>11\right]\end{array}$

where, π

_D^(Nk) is an object-environment distance weighted value of a node N_k, and d_f^(Nk) is a distance difference between the three-dimensional values of the fitted local feature points and the three-dimensional values of the same local feature points on the candidate node when the three-dimensional values of the local feature points of an object recognized by the robot and the environment around the object are fitted to the three-dimensional values of the local feature points of the same object on the candidate node and the environment around the object.

13. A method for self-localization of a robot, the robot including a camera unit formed by a stereo video device having a viewing angle smaller than 360°

, a rotating device rotating the camera unit by a predetermined angle, a database storing a map around a robot traveling path, and a position arithmetic unit estimating the position of the robot on the basis of image information acquired by the camera unit, the method comprising;

acquiring, in the camera unit, an image around the robot in a predetermined direction;

when no object is recognized in the image acquired by the camera unit, rotating, in the position arithmetic unit, the camera unit and repeatedly acquiring an image in another direction until an object is recognized in the acquired image, and generating position values on a camera coordinate system of the local feature points of the recognized individual objects and the local feature points of the surrounding environment including the individual objects;

selecting, in the position arithmetic unit, nodes on which at least recognized object is included, from the map stored in the database as candidate nodes,

Wherein the robot can be located on the candidate nodes;

giving a priority to each candidate node by the position arithmetic unit;

calculating, in the position arithmetic unit, a distance between objects on said each candidate node and the robot, and a highest priority object;

confirming, in the position arithmetic unit, the highest priority object on said each candidate node by rotating the camera unit; and

applying particle filtering to the respective candidate nodes having confirmed highest priority objects, thereby estimating the position of the robot, in the position arithmetic unit.

14. The method of claim 13, wherein estimating the position of the robot comprises, if a highest priority object in a candidate node is not recognized, confirming a highest priority object of a next highest priority candidate node from among the candidate nodes according to the given priority.