Self-positioning device and method thereof
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.
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.