Self-positioning device and method thereof

• US 8,385,683 B2
• Filed: 12/31/2009
• Issued: 02/26/2013
• Est. Priority Date: 10/26/2009
• Status: Active Grant

• Current Assignee: Industrial Technology Research Institute
• Original Assignee: Industrial Technology Research Institute
• Inventors: Lin, Tzung-Han
• Primary Examiner(s): Dulaney, Kathleen Y
• Assistant Examiner(s): Li, Ruiping
• Application Number:
  US12/650,553
• Publication Number:
  US 20110097014A1
• Time in Patent Office: 1,153 Days
• Field of Search: 382/284, 382/285, 382/173, 348/43, 348/47
• US Class Current: 382/284
• CPC Class Codes: G01C 15/002 Active optical surveying means optical plumbing G01C15/105
• CPC Class Codes: G01S 17/06 Systems determining position data of a target
• CPC Class Codes: G01S 17/86 Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
• CPC Class Codes: G01S 17/875 for determining attitude
• CPC Class Codes: G01S 17/89 for mapping or imaging

Abstract

A self-localization device and a method thereof are provided. The self-localization device has a movable carrier, a first laser image-taking device and a processor. The movable carrier can be moved and rotated on a plan. During the motion of the movable carrier, the first laser image-taking device disposed on the movable carrier acquires an i-th lot point data in the space at a time point t_i, where i is one index number from 1 to n, and n is an integer. The processor controls the first laser image-taking device, and receives coordinates of the i-th lot point data. The processor executes a K-D tree algorithm to perform a comparison and merge process between the first and the i-th lots point data, so as to establish a two dimensional profile.

Claims

1. A self-localization device, comprising:

a movable carrier, for performing a movement and a rotation on a plane, wherein the movement and the rotation are independent;

a laser image-taking device, disposed on the movable carrier, for acquiring an i-th lot point data in a space at a time point t_i when the movable carrier moves, where i is an index number from 1 to n, and n is an integer; and

a processor, controlling the laser image-taking device, and receiving coordinates of the i-th lot point data (iε

[1,n]),

wherein the processor executes a K-D tree algorithm to perform a comparison and merge process to a first and the i-th lots point data, so as to establish a two dimensional (2D) profile, and

the K-D tree algorithm refers to that the i-th lot point data (iε

[1,n]) obtained by the laser image-taking device is processed by the processor to generate a K-D tree structure, and a data comparison, a data merge or a carrier position update is performed between the K-D tree structure and the i-th lot point data obtained through a current scan.

2. The self-localization device as claimed in claim 1, wherein the comparison and merge process further comprises:

(i) establishing a K-D tree structure according to the first lot point data, wherein the K-D tree structure has a plurality of nodes, and each node consists of a plurality of point data;

(ii) comparing the i-th lot point data with each of the point data of each of the nodes in the K-D tree structure to find a point data closest to the i-th lot point data from each of the nodes to serve as a comparison point;

(iii) merging the i-th lot point data into the K-D tree structure; and

(iv) storing a current position and status of the movable carrier.

3. The self-localization device as claimed in claim 2, wherein the step (iii) further comprises:

distributing each of the i-th lot point data to a corresponding node in the K-D tree structure during merging.

4. The self-localization device as claimed in claim 2, wherein the step (iii) further comprises merging each of the point data according to a weighting function when each of the point data in each of the nodes is closed to a predetermined value.

5. The self-localization device as claimed in claim 2, wherein the step (iii) further comprises:

partitioning the node when a point density of each node of the K-D tree structure exceeds a predetermined value.

6. The self-localization device as claimed in claim 1, wherein the data comparison refers to that a global comparison is performed between the K-D tree structure and the i-th lot point data obtained through the current scan corresponding to a current position of the movable carrier, so as to determine a corresponding point, and a weighted iterative closest point (ICP) operation is performed to minimize a matching error between the i-th lot point data (iε

[1,n]) obtained through the current scan and a map data.

7. The self-localization device as claimed in claim 1, wherein the data merge refers to that the i-th lot point data is distributed one-by-one to a corresponding node in the K-D tree structure, and a weight linear merge is performed between the distributed point data and an adjacent data in the node, and a weight value of the merged point data is updated.

8. The self-localization device as claimed in claim 1, wherein the carrier position update refers to that while the i-th lot point data is merged to the K-D tree structure, a current position, status and time information of the movable carrier are recorded.

9. The self-localization device as claimed in claim 1, wherein the movable carrier comprises a wheel carrier or a non-wheel carrier.

10. The self-localization device as claimed in claim 9, wherein the wheel carrier comprises a single-wheel carrier or a multi-wheel carrier.

11. The self-localization device as claimed in claim 1, wherein the movable carrier comprises a device powered by fuel, electricity or a human-power.

12. The self-localization device as claimed in claim 9, wherein the non-wheel carrier comprises a track device, a ball-type device, a rail-type device or a magnetic levitation device.

13. A method for self-localization, comprising:

using a first laser image-taking device to acquire an i-th lot point data in a space at a time point t_i during a motion of a movable carrier, where i is an index number from 1 to n, and n is an integer;

using a processor to execute a K-D tree algorithm to perform a comparison and merge process to a first lot point data and the i-th lot point data obtained by the first laser image-taking device, so as to establish a 2D profile,

wherein the K-D tree algorithm refers to that the i-th lot point data (iε

[1,n]) obtained by the first laser image-taking device is processed by the processor to generate a K-D tree structure, and a data comparison, a data merge or a carrier position update is performed between the K-D tree structure and the i-th lot point data obtained through a current scan.

14. The method for self-localization as claimed in claim 13, wherein the comparison and merge process further comprises:

(i) establishing a K-D tree structure according to the first lot point data, wherein the K-D tree structure has a plurality of nodes, and each node consists of a plurality of point data;

(ii) comparing the i-th lot point data to each of the point data of each of the nodes in the K-D tree structure to find a point data closest to the i-th lot point data from each of the nodes to serve as a comparison point;

(iii) merging the i-th lot point data into the K-D tree structure; and

(iv) storing a current position and status of the movable carrier.

15. The method for self-localization as claimed in claim 14, wherein the step (iii) further comprises:

distributing each of the i-th lot point data to a corresponding node in the K-D tree structure during merging.

16. The method for self-localization as claimed in claim 15, wherein the step (iii) further comprises merging each of the point data according to a weight value when each of the point data in each of the nodes is closed to a predetermined value.

17. The method for self-localization as claimed in claim 14, wherein the step (iii) further comprises:

partitioning a node when a point density of the node of the K-D tree structure exceeds a predetermined value.

18. The method for self-localization as claimed in claim 13, wherein the data comparison refers to that a global comparison is performed between the K-D tree structure and the i-th lot point data obtained through the current scan corresponding to a current position of the movable carrier, so as to determine a corresponding point, and a weighted iterative closest point (ICP) operation is performed to minimize a matching error between the i-th lot point data (iε

[1,n]) obtained through the current scan and a map data.

19. The method for self-localization as claimed in claim 13, wherein the data merge refers to that the i-th lot point data is one-by-one distributed to a corresponding node in the K-D tree structure, and a weight linear merge is performed between the distributed point data and an adjacent data in the node, and a weight value of the merged point data is updated.

20. The method for self-localization as claimed in claim 13, wherein the carrier position update refers to that while the i-th lot point data is merged to the K-D tree structure, a current position, status and time information of the movable carrier are recorded.