Apparatus and method of controlling mobile body

• US 7,561,942 B2
• Filed: 08/23/2005
• Issued: 07/14/2009
• Est. Priority Date: 01/26/2005
• Status: Expired due to Fees

• Current Assignee: Samsung Electronics Co. Ltd.
• Original Assignee: Samsung Electronics Co. Ltd.
• Inventors: Kong, Donggeon, Choi, Changkyu
• Primary Examiner(s): Tran; Khoi
• Assistant Examiner(s): Marc; McDieunel
• Application Number:
  US11/208,849
• Publication Number:
  US 20060167588A1
• Time in Patent Office: 1,421 Days
• Field of Search: 700/245, 700/258, 700/259, 446/44, 446/46, 446/175, 446/409, 446/435, 446/436, 446/454, 446/460, 446/464, 701/23, 701/300
• US Class Current: 700/245
• CPC Class Codes: G05D 1/0255 using acoustic signals, e.g. ultra-sonic singals sonar systems designed for anti-collision purposes G01S15/93

Abstract

An apparatus and method of controlling a mobile body that travels around a sound source which generates a sound. This apparatus includes a traveling information producer for producing traveling information, which is information about traveling of the mobile body; a direction estimator for estimating a direction in which the mobile body is located with respect to the sound source; and a position determiner for determining a position of the mobile body using the traveling information and the estimated direction of the mobile body.

Claims

1. An apparatus for controlling a mobile body that travels around a sound source which generates a sound, the apparatus comprising:

a direction estimator estimating a direction in which the mobile body is located in a first position and a direction in which the mobile body is located in a second position with respect to the sound source;

a traveling information producer producing traveling information about traveling of the mobile body from the first position to the second position;

a traveling direction determiner determining a traveling direction of the mobile body using the estimated direction in the first position;

a traveling controller moving the mobile body a predetermined distance to the second position in the determined traveling direction; and

a position determiner determining a position of the mobile body using the traveling information and the estimated directions in the first and second positions.

2. The apparatus of claim 1, wherein the position of the mobile body includes a distance between the mobile body and the sound source, the distance being determined using the traveling information and the estimated direction.

3. The apparatus of claim 2, wherein the position determiner calculates the distance between the mobile body and the sound source using the following equation:

$l_2=l\times <br><br>\frac{\sin <br><br><br><br>\left({\mathrm{\theta <br><br>}}_1^{\mathrm{″<br><br>}}\right)}{\sin <br><br><br><br>\left({\mathrm{\theta <br><br>}}_2^{\mathrm{″<br><br>}}-{\mathrm{\theta <br><br>}}_1^{\mathrm{″<br><br>}}\right)},$

and

wherein l denotes a distance by which the mobile body travels from the first position to the second position and is included in the traveling information, l₂ denotes a distance between the second position to which the mobile body has traveled and the sound source, and θ

₁″ and

θ

₂″

denotes directions of the mobile body estimated by the direction estimator when the mobile body is located at the first and second positions with respect to the sound source.

4. The apparatus of claim 2, when the mobile body continuously travels from the first position to the second position, further comprising a position corrector correcting the distance between the second position to which the mobile body is moved and the sound source or the estimated direction of the mobile body that has reached the second position using a power distribution of the sound and a triangonometry,

wherein the position of the mobile body includes the distance or the estimated direction.

5. The apparatus of claim 4, when the position corrector comprises:

a real position search unit searching for a real position for the determined position of the mobile body on a path between a third position where the direction of the mobile body starts being estimated and a second position where the estimation of the direction of the mobile body ends, using the power distribution of the sound;

a distance error calculation unit calculating a distance between the real position and the second position and outputting the distance as a distance error;

a distance calculation unit calculating a distance between the real position and the sound source by triangonometry using a result obtained by subtracting the distance error from the traveling distance between the first and second positions, the direction of the mobile body located at the first position, and the estimated direction of the mobile body located at the second position;

a distance correction unit correcting the distance calculated by the distance calculation unit using the distance error calculated by the distance error calculation unit and the estimated direction of the mobile body at the second position; and

a direction correction unit correcting the estimated direction of the mobile body located at the second position using a ratio between the corrected distance and the calculated distance.

6. The apparatus of claim 4, when the real position a position on the path between the third and second positions where the sound power satisfies the following equation:

$\frac{\underset{n=N-N_2+1}{\overset{N}{\sum <br><br>}}<br><br>P<br><br>\left(n\right)}{\frac{1}{N}<br><br>\underset{n=1}{\overset{N}{\sum <br><br>}}<br><br>P<br><br>\left(n\right)}=0.5$

, and

wherein N denotes a number of frames existing between the third and second positions and is N₁+N₂, N₁ denotes a number of frames existing between the third position and the real position, N₂ denotes a number of frames existing between the second position and the real position, and P(n) denotes a power of each of the frames.

7. The apparatus of claim 5, when the distance error calculation unit calculates the distance error using the following equation:

$l_{N<br><br><br><br>2}=\frac{N_2}{N}<br><br>l_N,$

and

wherein l_N denotes a distance between the third and second positions by which the mobile body travels, and l_N2 denotes the distance error.

8. The apparatus of claim 5, wherein:

the distance calculation unit calculates the distance between the real position and the sound source using the following equation;

$l_2^{\mathrm{\prime <br><br>}}=\left(l-l_{N_2}\right)<br><br>\frac{\sin <br><br><br><br>{\mathrm{\theta <br><br>}}_1^{\mathrm{″<br><br>}}}{\sin <br><br><br><br>\left({\mathrm{\theta <br><br>}}_2^{\mathrm{″<br><br>\prime <br><br>}}-{\mathrm{\theta <br><br>}}_1^{\mathrm{″<br><br>}}\right)}$

in which l₂′

denotes the distance between the real position and the sound source, l denotes a traveling distance between the first and second positions, l_N2 denotes the distance error, θ

″

₂ denotes the estimated direction of the mobile body located at the first position, and θ

′

″

₂ denotes the direction of the mobile body estimated at the second position;

the distance correction unit corrects the distance calculated by the distance calculation unit using the following equation;

$l_2=\sqrt{l_{N_2}^2+{l^{\mathrm{\prime <br><br>}}}_2^2-2<br><br>l_{N_2}<br><br>l_2^{\mathrm{\prime <br><br>}}<br><br>\cos <br><br>\left({\mathrm{\theta <br><br>}}_2^{\mathrm{\prime <br><br>\prime <br><br>\prime <br><br>}}\right)}$

in which l₂ denotes the corrected distance; and

the direction correction unit corrects the estimated direction of the mobile body using the following equation;

${\mathrm{\theta <br><br>}}_2^{\mathrm{\prime <br><br>\prime <br><br>}}=\mathrm{asin}<br><br>\left(\sin <br><br>\left({\mathrm{\theta <br><br>}}_2^{\mathrm{\prime <br><br>\prime <br><br>\prime <br><br>}}\right)<br><br>\frac{l_2^{\mathrm{\prime <br><br>}}}{l_2}\right)$

in which θ

″

₂ denotes the corrected direction.

9. The apparatus of claim 1, further comprising a sound detector determining whether a predetermined sound has been detected and outputting a result of the determination as a sound detection signal,

wherein the direction estimator estimates the direction of the mobile body in response to the sound detection signal.

10. The apparatus of claim 9, wherein the sound detector comprises:

a noise removal unit removing noise from the generated sound;

a component extraction unit extracting a signal component at a level at least equal to a predetermined level from a result of the noise removal; and

a sound recognition unit determining whether the extracted signal component is the predetermined sound and outputting a result of the determination as the sound detection signal.

11. The apparatus of claim 9, wherein the sound detector comprises:

a band pass filter filtering a predetermined band component from the generated sound;

an envelope detection unit detecting an envelope corresponding to a result of the filtering; and

a level comparison unit comparing a level of the detected envelope with a critical level and outputting a result of the comparison as the sound detection signal.

12. The apparatus of claim 9, wherein the sound detector comprises:

a first band pass filter filtering a first predetermined band component out of the generated sound;

a second band pass filter filtering a second predetermined band component out of the generated sound;

a first envelope detection unit detecting an envelope of a result of the band pass filtering by the first band pass filter;

a second envelope detection unit detecting an envelope of a result of the band pass filtering by the second band pass filter;

a first level comparison unit comparing a level of the envelope detected by the first envelope detection unit with a first critical level;

a second level comparison unit comparing a level of the envelope detected by the second envelope detection unit with a second critical level; and

a level recognition unit determining whether the first and second predetermined band components are alternately detected using the results of the comparisons by the first and second level comparison units and outputting a result of the determination as the sound detection signal.

13. The apparatus of claim 1, wherein the traveling controller moves the mobile body toward the sound source using the estimated direction and the determined distance.

14. The apparatus of claim 1, further comprising a comparator comparing the determined distance with a critical value,

wherein the traveling direction determiner renews the traveling direction in response to a result of the comparison by the comparator.

15. The apparatus of claim 1, wherein the position determiner predicts a current position of the mobile body using the traveling information, corrects the predicted current position using the estimated direction, and outputs a result of the correction as the determined position of the mobile body.

16. The apparatus of claim 15, wherein the position determiner comprises:

a direction prediction unit predicting a direction of the mobile body from the predicted position of the mobile body and outputting the predicted direction of the mobile body;

a direction error production unit subtracting the predicted direction from the estimated direction and outputting a result of the subtraction as a direction error value;

a position correction unit correcting the predicted position of the mobile body using the direction error value and outputting a result of the correction as the determined position of the mobile body;

a time delay unit delaying the result of the correction; and

a position prediction unit predicting the position of the mobile body from the result of the delay and the traveling information.

17. The apparatus of claim 16, wherein the traveling information is at least one of a traveling velocity of the mobile body, a directional angular velocity of the mobile body, and a traveling distance of the mobile body.

18. The apparatus of claim 17, wherein the position correction unit corrects the predicted position of the mobile body using the direction error value defined by the following equation:

m(k|k)={circumflex over (m)}(k|k−

1)+K(k)[θ

(k)−

h({circumflex over (m)}(k|k−

1))]

in which m(k|k) denotes a result obtained by estimating the state of m(k) at current time k, m(k) denotes a determined position of the mobile body at current time k, {circumflex over (m)}(k|k−

1) denotes the predicted position of the mobile body, θ

(k)−

h({circumflex over (m)}(k|k−

1)) denotes the direction error value, θ

(k) denotes the estimated direction of the mobile body, h({circumflex over (m)}(k|k−

1)) denotes the predicted direction of the mobile body, K(k) denotes a Kalman gain and is defined by;

K(k)={circumflex over (P)}(k|k−

1)H^T(k)R_e<sup>−

1</sup>(k), {circumflex over (P)}(k|k−

1)

is equal to FP(k−

1|k−

1)F^T+Q, F(k) is defined by;

$F<br><br>\left(k\right)=\frac{\partial <br><br>f<br><br>\left(u<br><br>\left(k\right)\right)}{\partial <br><br>m},u<br><br>\left(k\right)$

denotes the traveling information, P(k|k) is defined by;

P(k|k)={circumflex over (P)}(k|k−

1)−

{circumflex over (P)}(k|k−

1)H^T(k)R_e⁻

(k)H(k){circumflex over (P)}(k|k−

1),_T

denotes a transpose, Q denotes a process noise covariance matrix and is defined by;

Q=E[w*w^H],

w denotes process noise, H(k) is defined by;

$H<br><br>\left(k\right)=\frac{\partial <br><br>h<br><br>\left(m<br><br>\left(k\right)\right)}{\partial <br><br>m},$

R_e(k) is defined by;

R_e(k)=R+H(k){circumflex over (P)}(k|k−

1)H^T(k),

R denotes a measurement noise covariance matrix and is given by;

R=E[v* v^H], and v denotes measurement noise.

19. The apparatus of claim 1, wherein the direction estimator comprises:

a real number transformation unit receiving a multi-channel sound expressed as a complex value and transforming the complex value into a real number;

a sound estimation unit estimating the sound using the multi-channel sound expressed as the complex value and a predicted state vector of the mobile body;

a sound prediction unit predicting the sound using the sound estimated by the sound estimation unit and the predicted state vector of the mobile body;

a sound error production unit subtracting the sound predicted by the sound prediction unit from a result of the transformation by the real number transformation unit and outputting a result of the subtraction as a sound error value;

a state vector correction unit correcting the predicted state vector of the mobile body using the sound error value and the estimated sound received from the sound estimation unit and outputting the estimated direction of the mobile body from a result of the correction;

a time delay unit delaying the result of the correction; and

a state vector prediction unit predicting the state vector using the result of the delay and outputting the predicted state vector.

20. The apparatus of claim 19, wherein:

the sound estimation unit estimates the sound using the following equation;

ŝ

(k)=(A^H({circumflex over (θ

)}(k|k−

1))A({circumflex over (θ

)}(k|k−

1)))<sup>−

1</sup>A^H({circumflex over (θ

)}(k|k−

1))z_array(kT)

in which ŝ

(k) denotes the sound estimated by the sound estimation unit, A(θ

(t)) is given by;

A((θ

(t))=[a(θ

(t))], a denotes a steering vector, H denotes Hermition, z_array(kT) denotes the multi-channel sound expressed as a complex value, and T denotes a sampling time;

the sound prediction unit predicts the sound using the following equation;

$\hat{z}<br><br>\left(k|k-1\right)=h<br><br>\left({\hat{x}}^{\mathrm{\prime <br><br>}}<br><br>\left(k|k-1\right),\hat{s}<br><br>\left(k\right)\right)=\left[\begin{array}{c}\mathrm{Real}<br><br><br><br>\left\{A<br><br>\left(\widehat{\mathrm{\theta <br><br>}}<br><br>\left(k|k-1\right)\right)<br><br>\hat{s}<br><br>\left(k\right)\right\}\\ \mathrm{Image}<br><br><br><br>\left\{A<br><br>\left(\widehat{\mathrm{\theta <br><br>}}<br><br>\left(k|k-1\right)\right)<br><br>\hat{s}<br><br>\left(k\right)\right\}\end{array}\right]$

in which {circumflex over (x)}{circumflex over (′

)}(k|k−

1) is the predicted state vector, x′

(k) denotes a state vector and is equal to

$\left[\mathrm{\theta <br><br>}<br><br>\left(k\right),\frac{d<br><br>\mathrm{\theta <br><br>}<br><br>\left(k\right)}{d<br><br>k}\right],\hat{z}<br><br>\left(k❘<br><br>k-1\right)$

is the predicted sound, z(kT) is equal to

$\left[\begin{array}{c}\mathrm{Real}<br><br><br><br>\left(z_{\mathrm{array}}<br><br>\left(\mathrm{kT}\right)\right)\\ \mathrm{Image}<br><br><br><br>\left(z_{\mathrm{array}}<br><br>\left(\mathrm{kT}\right)\right)\end{array}\right],$

Real{i} denotes a real number of i, Image{i} denotes an imaginary number of l; and

the state vector correction unit corrects the predicted state vector using the following equation;

x′

(k|k)={circumflex over (x)}{circumflex over (′

)}(k|k−

1)+K′

(k)[z(k)−

{circumflex over (z)}(k|k−

1)]

in which x′

(k|k) denotes the state vector corrected by the state vector correction unit, z(k)−

{circumflex over (z)}(k|k−

1) denotes the sound error value produced by the sound error production unit, K′

(k) denotes a Kalman gain and is defined by;

K′

(k)={circumflex over (P)}{circumflex over (′

)}(k|k−

1)H′

^T(k,ŝ

(k))R′

_e<sup>−

1</sup>(k), {circumflex over (P)}{circumflex over (′

)}(k|k−

1)

is defined by;

{circumflex over (P)}{circumflex over (′

)}(k|k−

1)=F′

P′

(k−

1|k−

1)F′

^T+G′

Q′

G′

^T,

F′

is given by;

$F^{\mathrm{\prime <br><br>}}=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right],G^{\mathrm{\prime <br><br>}}$

is given by;

$G^{\mathrm{\prime <br><br>}}=\left[\begin{array}{cc}\frac{1}{2}<br><br>T^2& 0\\ 0& T\end{array}\right],P^{\mathrm{\prime <br><br>}}<br><br><br><br>\left(k❘<br><br>k\right)$

is defined by

P′

(k|k)={circumflex over (P)}{circumflex over (′

)}(k|k−

1)−

{circumflex over (P)}{circumflex over (′

)}(k|k−

1)H′

^T(k,ŝ

(k))R′

_e<sup>−

1</sup>(k)H′

(k,ŝ

(k)){circumflex over (P)}{circumflex over (′

)}(k|k−

1)

by;

$H^{\mathrm{\prime <br><br>}}<br><br><br><br>\left(k,\hat{s}<br><br>\left(k\right)\right)$

is equal to

$\left[\begin{array}{c}\mathrm{Real}<br><br>\left(\frac{\partial <br><br>}{\partial <br><br>x^{\mathrm{\prime <br><br>}}}<br><br>\left(A<br><br>\left(\mathrm{\theta <br><br>}<br><br>\left(k\right)\right)<br><br>\hat{s}<br><br>\left(k\right)\right)\right)\\ \mathrm{Image}<br><br>\left(\frac{\partial <br><br>}{\partial <br><br>x^{\mathrm{\prime <br><br>}}}<br><br>\left(A<br><br>\left(\mathrm{\theta <br><br>}<br><br>\left(k\right)\right)<br><br>\hat{s}<br><br>\left(k\right)\right)\right)\end{array}\right],$

R′

_e(k) is defined by;

R′

_e(k)=R′

+H′

(k,ŝ

(k)){circumflex over (P)}{circumflex over (′

)}(k|k−

1)H′

^T(k,ŝ

(k))

21. A method of controlling a mobile body that travels around a sound source which generates a sound, comprising:

estimating a direction in which the mobile body is located in a first position with respect to the sound source;

determining a traveling direction of the mobile body using the estimated direction of the mobile body;

moving the mobile body a predetermined distance to a second position in the determined travelling direction;

producing traveling information about the traveling of the mobile body;

estimating a direction in which the mobile body is located in the second position with respect to the sound source after the mobile body has moved; and

determining a position of the mobile body using the traveling information and the estimated directions in the first and second positions.

22. The method of claim 21, wherein the determined position includes a distance between the mobile body and the sound source determined using the traveling information and the estimated directions.

23. The method of claim 21, further comprising:

determining whether a predetermined sound has been detected and proceeding to the operation of estimating the direction of the mobile body when it is determined that the predetermined sound has been detected; and

after performing the operation of producing the traveling information, determining whether the predetermined sound has been detected and proceeding to the operation of estimating the direction of the moved mobile body when it is determined that the predetermined sound has been detected.

24. The method of claim 21, further comprising moving the mobile body toward the sound source using the estimated direction and the determined distance.

25. The method of claim 21, further comprising:

determining whether the determined distance is smaller than a critical value; and

renewing the traveling direction using the estimated direction and proceeding to the operation of moving the mobile body by the predetermined distance, when it is determined that the determined distance is at least equal to the critical value.

26. The method of claim 21, when the mobile body continuously travels to the second position from the first position, further comprising correcting the distance between the mobile body at the second position and the sound source or the estimated direction of the mobile body traveled to the second position using a power distribution of the sound and a triangonometry.

27. The method of claim 26, wherein the correcting comprises:

searching for a real position for the predetermined position of the mobile body on a path between a third position, where a direction of the mobile body starts being estimated, and the second position, where the estimation of the direction of the mobile body ends, using the power distribution of the sound;

calculating a distance between the real position and the second position and determining the distance as a distance error;

calculating a distance between the real position and the sound source by triangonometry using a result obtained by subtracting the distance error from the traveling distance between the first and second positions, the estimated direction of the mobile body at the first position, and the estimated direction of the mobile body moved at the second position;

correcting the determined distance using the calculated distance between the real position and the sound source, the distance error, and the estimated direction of the mobile body traveled to the second position; and

correcting the estimated direction of the mobile body located at the second position using a ratio between the corrected distance and the calculated distance,

wherein the position of the mobile body includes at least one of the distance and the estimated direction.

28. The method of claim 21, wherein the determining of the position of the mobile body comprises predicting a current position of the mobile body using the traveling information, correcting the predicted current position using the estimated direction, and determining the corrected position as the position of the mobile body.

29. A computer-readable storage medium encoded with processing instructions for causing a processor to perform a method of controlling a mobile body that travels around a sound source which generates a sound, the method comprising:

estimating a direction in which the mobile body is located in a first position with respect to the sound source

determining a traveling direction of the mobile body using the estimated direction of the mobile body;

moving the mobile body a predetermined distance to a second position in the determined travelling direction;

producing traveling information about the traveling of the mobile body;

estimating a direction in which the mobile body is located in the second position with respect to the sound source after the mobile body has moved; and

determining a position of the mobile body using the traveling information and the estimated directions in the first and second positions.