SELF-POSITIONING DEVICE AND METHOD THEREOF

• US 20110097014A1
• Filed: 12/31/2009
• Published: 04/28/2011
• 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
• Granted Patent: US 8,385,683 B2
• Time in Patent Office: Days
• 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 plan, 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.

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 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.

7. The self-localization device as claimed in claim 6, 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.

8. The self-localization device as claimed in claim 6, 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.

9. The self-localization device as claimed in claim 6, 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.

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

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

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

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

14. A self-localization device, comprising:

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

a first laser image-taking device, disposed on the movable carrier, for acquiring an i-th lot point data along a first direction in a space at a time point t, during a motion of the movable carrier, where i is an index number from 1 to n, and n is an integer;

at least one second laser image-taking device, disposed on the movable carrier, for acquiring a j-th lot point data along a second direction in the space at a time point t_j (j is an index number from 1 to m, and m is an integer); and

a processor, controlling the first and the second laser image-taking devices, and receiving coordinates of the i-th lot point data obtained by the first laser image-taking device and coordinates of the j-th lot point data obtained by the second laser image-taking device (iε

[1,n], jε

[1,m]),

wherein the processor executes 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, and

restores a 3D map according to the 2D profile and the j-th lot point data obtained by the second laser image-taking device.

15. The self-localization device as claimed in claim 14, 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.

16. The self-localization device as claimed in claim 15, 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.

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

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

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

19. The self-localization device as claimed in claim 14, 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.

20. The self-localization device as claimed in claim 19, 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.

21. The self-localization device as claimed in claim 19, 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.

22. The self-localization device as claimed in claim 19, 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.

23. The self-localization device as claimed in claim 14, wherein the processor calculates a relative position and a relative status of the first and the second laser image-taking devices at any time point according to a synchronization algorithm or an interpolation algorithm.

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

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

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

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

28. The self-localization device as claimed in claim 14, further comprising an image-taking device.

29. The self-localization device as claimed in claim 28, wherein the image-taking device comprises a sonar, a black and white charge-coupled device (CCD), or a color CCD.

30. 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.

31. The method for self-localization as claimed in claim 30, 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.

32. The method for self-localization as claimed in claim 31, 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.

33. The method for self-localization as claimed in claim 32, 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.

34. The method for self-localization as claimed in claim 31, 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.

35. The method for self-localization as claimed in claim 30, 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.

36. The method for self-localization as claimed in claim 35, 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.

37. The method for self-localization as claimed in claim 35, 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.

38. The method for self-localization as claimed in claim 35, 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.

39. The method for self-localization as claimed in claim 30, further comprising:

using at least a second laser image-taking device to acquire a j-th lot point data along a second direction in the space at a time point t_j, where j is one index number from 1 to m, and m is an integer; and

restoring a 3D map according to the 2D profile and the j-th lot point data (jε

[1,m]) obtained by the second laser image-taking device.

40. The method for self-localization as claimed in claim 30, further comprising using a synchronization algorithm or an interpolation algorithm to calculate a relative position and a relative status of the first and the second laser image-taking devices at any time point.