Method and apparatus for estimating 3D position and orientation through sensor fusion

• US 9,759,804 B2
• Filed: 05/07/2012
• Issued: 09/12/2017
• Est. Priority Date: 06/22/2011
• Status: Active Grant

• Current Assignee: Samsung Electronics Co. Ltd.
• Original Assignee: Samsung Electronics Co. Ltd.
• Inventors: Lee, Hyong Euk, Kim, Sang Hyun, Bang, Won Chul, Choi, Chang Kyu
• Primary Examiner(s): Le, Toan
• Assistant Examiner(s): Sun, Xiuquin
• Application Number:
  US13/465,428
• Publication Number:
  US 20120330594A1
• Time in Patent Office: 1,954 Days
• Field of Search: 702 94
• CPC Class Codes: G01S 5/16 using electromagnetic waves other than radio waves
• CPC Class Codes: G01S 5/163 Determination of attitude using inertial means G01C9/00; control of attitude G05D1/49

Abstract

An apparatus and method for estimating a three-dimensional (3D) position and orientation based on a sensor fusion process is provided. The method of estimating the 3D position and orientation may include estimating a strength-based position and a strength-based orientation of a remote apparatus when a plurality of strength information is received, based on an attenuation characteristic of a strength that varies based on a distance and orientation, estimating an inertia-based position and an inertia-based orientation of the remote apparatus by receiving a plurality of inertial information, and estimating a fused position based on a weighted-sum of the strength-based position and the inertia-based position, and to estimate a fused orientation based on a weighted-sum of the strength-based orientation and the inertia-based orientation. The strength-based position and the strength-based orientation may be estimated based on a plurality of adjusted strength information from which noise is removed using a plurality of previous strength information.

Claims

1. An apparatus for estimating a position and an orientation, the apparatus comprising:

a strength-based estimating processor configured to estimate a strength-based position and a strength-based orientation of a remote apparatus in response to a plurality of strength information being received, based on an attenuation characteristic of a strength that varies based on a distance and an orientation;

an inertia-based estimating processor configured to estimate an inertia-based position and an inertia-based orientation of the remote apparatus by receiving a plurality of inertial information; and

a fusion estimating processor configured to;

in response to a decrease in a movement speed of the remote apparatus or an increase in a distance from the remote apparatus to a receiving apparatus, assign a greater weight to the inertia-based position than a weight assigned to the strength-based position and assign a greater weight to the inertia-based orientation than a weight assigned to the strength-based orientation,

estimate a fused position based on a weighted-sum of the strength-based position and the inertia-based position,

estimate a fused orientation based on a weighted-sum of the strength-based orientation and the inertia-based orientation, and

in response to the movement speed of the remote apparatus being lower than a predetermined speed, estimate the fused position to be the inertia-based position and the fused orientation to be the inertia-based orientation; and

a strength adjusting processor configured to adjust, based on the fused position and the fused orientation, a plurality of subsequently received strength information,

wherein the plurality of strength information corresponds to a strength of a first optical signal, of a first frequency, emitted from a first transmitter and a strength of a second optical signal, of a second frequency, emitted from a second transmitter, and

wherein the first and second transmitters have different directionalities from each other, a predetermined angle between the directionalities is determined in advance, and the first and second optical signals are emitted simultaneously.

2. The apparatus of claim 1, further comprising:

a raw data estimating processor configured to estimate a new plurality of strength information based on the fused position and the fused orientation;

a noise estimating processor configured to estimate noise by comparing the plurality of estimated strength information and the plurality of received strength information, wherein

the strength adjusting processor is further configured to adjust the plurality of subsequently received strength information by removing the noise, and to provide the plurality of adjusted subsequently received strength information to the strength-based estimating processor, and

wherein the strength-based estimating processor is configured to estimate, based on the plurality of adjusted strength information, the strength-based position and the strength-based orientation in response to the plurality of adjusted strength information being received.

3. The apparatus of claim 1, wherein the fusion estimating processor is configured to calculate the weighted-sums by assigning the weight to the strength-based position and the strength-based orientation and assigning the greater weight to the inertia-based position and the inertia-based orientation, as the movement speed of the remote apparatus decreases.

4. The apparatus of claim 1, wherein the fusion estimating processor is configured to calculate the weighted-sums by assigning the weight to the strength-based position and the strength-based orientation and assigning the greater weight to the inertia-based position and the inertia-based orientation, as the distance from the remote apparatus to the receiving apparatus increases.

5. The apparatus of claim 1, wherein the inertia-based estimating processor is configured to estimate, based on a latest estimated fused position or a latest estimated strength-based position, the inertia-based position and the inertia-based orientation using a variance in the plurality of inertial information.

6. The apparatus of claim 5, wherein the fusion estimating processor is configured to:

estimate the strength-based position and the strength-based orientation as the fused position and the fused orientation, respectively, at a point in time in which the strength-based position and the strength-based orientation exist; and

estimate the inertia-based position and the inertia-based orientation as the fused position and the fused orientation, respectively, at a point in time in which the strength-based position and the strength-based orientation are absent.

7. The apparatus of claim 1, wherein at least one of the strength-based estimating processor, the inertia-based estimating processor, and the fusion estimating processor is configured as a Kalman Filter.

8. The apparatus of claim 1, wherein the plurality of inertial information includes at least one of a tri-axial acceleration, a tri-axial gravity acceleration, and a tri-axial angular velocity measured through an inertial sensor.

9. The apparatus of claim 1, wherein a minimum combination of transmitters and receivers for estimating a portion of the strength-based position and the strength-based orientation comprises at least:

a combination of one transmitter and at least three receivers, a combination of two transmitters and at least two receivers, or a combination of three transmitters and at least one receiver.

10. A method of estimating a position and an orientation, the method comprising:

estimating, using a processor, a strength-based position and a strength-based orientation of a remote apparatus in response to a plurality of strength information being received, using an attenuation characteristic of a strength that varies based on a distance and an orientation;

estimating, using the processor, an inertia-based position and an inertia-based orientation of the remote apparatus by receiving a plurality of inertial information;

in response to a decrease in a movement speed of the remote apparatus or an increase in a distance from the remote apparatus to a receiving apparatus, assigning a greater weight to the inertia-based position than a weight assigned to the strength-based position and assigning a greater weight to the inertia-based orientation than a weight assigned to the strength-based orientation;

estimating, using the processor, a fused position by calculating a weighted-sum of the strength-based position and the inertia-based position;

estimating, using the processor, a fused orientation by calculating a weighted-sum of the strength-based orientation and the inertia-based orientation;

in response to the movement speed of the remote apparatus being lower than a predetermined speed, estimating the fused position to be the inertia-based position and the fused orientation to be the inertia-based orientation; and

adjusting, based on the fused position and the fused orientation, a plurality of subsequently received strength information,

wherein the plurality of strength information corresponds to a strength of a first optical signal, of a first frequency, emitted from a first transmitter and a strength of a second optical signal, of a second frequency, emitted from a second transmitter, and

wherein the first and second transmitters have different directionalities from each other, a predetermined angle between the directionalities is determined in advance, and the first and second optical signals are emitted simultaneously.

11. The method of claim 10, further comprising:

reversely estimating a plurality of strength information based on the fused position and the fused orientation;

estimating noise by comparing the plurality of estimated strength information and the plurality of received strength information; and

wherein the adjusting of the plurality of subsequently received strength information comprises removing the noise, and outputting the plurality of adjusted subsequently received strength information, and

wherein the estimating of the strength-based position and the strength-based orientation comprises;

estimating the strength-based position and the strength-based orientation based on a plurality of adjusted strength information.

12. The method of claim 10, wherein the calculating of the weighted-sums comprises assigning a weight to the strength-based position and the strength-based orientation and a greater weight to the inertia-based position and the inertia-based orientation, as the movement speed of the remote apparatus decreases.

13. The method of claim 10, wherein the calculating of the weighted-sums comprises assigning a weight to the strength-based position and the strength-based orientation and a greater weight to the inertia-based position and the inertia-based orientation, as the distance from the remote apparatus to the receiving apparatus increases.

14. The method of claim 10, wherein the plurality of inertial information include at least one of a tri-axial acceleration, a tri-axial gravity acceleration, and a tri-axial angular velocity measured through an inertial sensor.

15. A system of estimating a position and an orientation, the system comprising:

a transmitting apparatus comprising a first transmitter and a second transmitter configured to transmit a signal for measuring a strength;

a receiving apparatus comprising at least two receivers configured to receive the signal, and to measure the strength of the signal, and outputting a plurality of strength information;

a remote apparatus comprising an inertia sensor unit configured to measure a plurality of inertial information, and one of the transmitting apparatus and the receiving apparatus;

an estimating processor configured to estimate a fused position and a fused orientation of the remote apparatus by fusing the plurality of strength information and the plurality of inertial information; and

a strength adjusting processor configured to adjust, based on a fused position and a fused orientation, a plurality of subsequent strength information output by the receiving apparatus,

wherein the estimating processor comprises a fusion estimating processor configured to;

in response to a decrease in a movement speed of the remote apparatus or an increase in a distance from the remote apparatus to the receiving apparatus, assign a greater weight to the inertia-based position than a weight assigned to the strength-based position and assign a greater weight to the inertia-based orientation than a weight assigned to the strength-based orientation,

estimate the fused position based on a weighted-sum of a strength-based position and an inertia-based position,

estimate the fused orientation based on a weighted-sum of a strength-based orientation and an inertia-based orientation, and

estimate, in response to the movement speed of the remote apparatus being lower than a predetermined speed, the fused position to be the inertia-based position and the fused orientation to be the inertia-based orientation,

wherein the plurality of strength information corresponds to a strength of a first optical signal, of a first frequency, emitted from the first transmitter and a strength of a second optical signal, of a second frequency, emitted from the second transmitter, and

wherein the first and second transmitters have different directionalities from each other, a predetermined angle between the directionalities is determined in advance, and the first and second optical signals are emitted simultaneously.

16. The system of claim 15, wherein the estimating processor comprises:

a strength-based estimating processor configured to estimate the strength-based position and the strength-based orientation of the remote apparatus in response to the plurality of strength information being received, based on an attenuation characteristic of a strength that varies based on a distance and orientation; and

an inertia-based estimating processor configured to estimate the inertia-based position and the inertia-based orientation of the remote apparatus by receiving the plurality of inertial information.

17. The system of claim 16, wherein:

the estimating processor further comprises;

a raw data estimating processor configured to estimate a new plurality of strength information based on the fused position and the fused orientation;

a noise estimating processor configured to estimate noise by comparing the plurality of estimated strength information and the plurality of received strength information,

wherein the strength adjusting processor is configured to adjust a plurality of subsequent strength information by removing the noise, and to provide the plurality of adjusted strength information to the strength-based estimating processor,

wherein the strength-based estimating processor is configured to estimate, based on a plurality of adjusted strength information, the strength-based position and the strength-based orientation in response to the plurality of adjusted strength are received.

18. The system of claim 15, wherein:

at least three receivers are included in the receiving apparatus in response to two transmitters being used, and

at least two receivers are included in the receiving apparatus in response to three transmitters being used.

19. The system of claim 15, wherein positions and directionalities of the at least two receivers are determined in advance.

20. The system of claim 15, wherein:

the at least two transmitters are configured to transmit the signal based on a predetermined frequency that is robust against noise, and

the at least two receivers are configured to receive the signal using a filter through which the predetermined frequency is able to pass.

21. The apparatus of claim 15, wherein, in response to using the at least two transmitters, the at least one receiver is configured to synchronize with the transmitting apparatus upon receiving a synchronization signal, before receiving a signal from the transmitting apparatus.