A positioning method and device based on point cloud data

Abstract

The present invention discloses a positioning method and device based on point cloud data. The method includes the following steps: S1, collecting a circle of point cloud data as target point cloud data; S2, obtaining the shortest distance between each coordinate point in the target point cloud data and the template point cloud data, and calculating a displacement estimate based on the shortest distance; S3, calculating the current position of the robot based on the displacement estimate and the initial position of the robot. The positioning method of the present invention calculates a displacement estimate using point cloud data to achieve positioning. During the process, it is not necessary to use the data collected by GPS and vision sensors. Therefore, it can be applied in a "double-blind" scenario. Moreover, this method does not need to use the point cloud map commonly used in existing positioning technologies. The point cloud map is the fusion of multiple frames of point cloud data, while this method only needs to use a single frame of point cloud data to achieve positioning. Therefore, the amount of calculated data is smaller than that of existing positioning technologies, and it can run on low-cost platforms with low computing power such as single-chip microcomputers.

Description

Technical Field

[0001] The present invention relates to the field of robots, and particularly to a positioning method and device based on point cloud data. Background Art

[0002] In recent years, micro and small unmanned aerial vehicles (UAVs) have developed rapidly and are widely used in real life due to their flexibility, strong maneuverability, and low cost. The navigation means usually adopted by UAVs during autonomous flight is to estimate the navigation state by fusing GPS, vision sensors, lidar, and inertial sensors. With the expansion of the application scope, the application demand of UAVs in the "double-blind" environment without GPS and without light is gradually increasing. The positioning methods of the existing technologies generally need to utilize means such as GPS and visual navigation, but the GPS and visual navigation means will fail in the "double-blind" environment. Therefore, there is an urgent need for a positioning method that does not require GPS and vision sensors. Summary of the Invention

[0003] The purpose of the present invention is to provide a positioning method and device based on point cloud data, which can achieve positioning in an environment without GPS and without light.

[0004] To achieve the above object, the present invention adopts the following technical solutions:

[0005] A positioning method based on point cloud data includes the following steps:

[0006] S1. Collect a circle of point cloud data as target point cloud data;

[0007] S2. Obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data, and calculate the displacement estimation value based on the shortest distance;

[0008] S3. Calculate the current position of the robot based on the displacement estimation value and the initial position of the robot.

[0009] Optionally, the step S2 includes:

[0010] S21. Obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data;

[0011] S22. Calculate the displacement estimation value based on the shortest distance;

[0012] S23. Calculate the offset based on the displacement estimation value;

[0013] S24. Compare whether the offset is less than a preset offset threshold. If so, execute step S3; if not, update the target point cloud data based on the displacement estimation value, and then return to step S21.

[0014] Optionally, before the step S1, it further includes:

[0015] Collect a circle of point cloud data as the template point cloud data;

[0016] Perform data preprocessing on the collected template point cloud data;

[0017] Store the template point cloud data after data preprocessing in the form of a KD - Tree;

[0018] The step S1 includes:

[0019] S11. Collect a circle of point cloud data as the target point cloud data;

[0020] S12. Perform data preprocessing on the collected target point cloud data;

[0021] Before the step S1, it also includes:

[0022] Initialize the initial position of the robot (x 0 ,y 0 ) to (0,0) ;

[0023] When collecting a circle of point cloud data as the template point cloud data, the robot is in a stationary state.

[0024] Optionally, the data format of the point cloud data is (r, θ + φ) , where r is the distance value collected by the lidar, θ is the scanning angle value of the lidar, φ is the rotation angle value collected by the attitude sensor;

[0025] The data preprocessing includes:

[0026] Filter the collected point cloud data to remove r < 3 or r = 0 coordinate points;

[0027] Perform coordinate transformation on the filtered point cloud data, and transform the r , θ and φ in polar coordinates to x and y in Cartesian coordinates. The transformation relationship includes:

[0028] ;

[0029] ;

[0030] where the subscript i represents the i th coordinate point in the point cloud data,N Indicates the total number of coordinate points in the point cloud data;

[0031] Perform a centering process on the point cloud data after coordinate transformation. The calculation formulas include:

[0032] ;

[0033] ;

[0034] ;

[0035] ;

[0036] where the subscript i represents the i th coordinate point in the point cloud data, N represents the total number of coordinate points in the point cloud data;

[0037] The data format of the point cloud data after the data preprocessing is a two-dimensional array (x ci ,y ci ) ; The template point cloud data is identified by the superscript 0 The target point cloud data is identified by the superscript 1 ;

[0038] Optionally, before step S21, it further includes:

[0039] S20. Initialize the displacement estimate (Δx, Δy) The calculation formulas include:

[0040] ;

[0041] ;

[0042] Step S21 includes:

[0043] Retrieve each coordinate point in the target point cloud data in the template point cloud data stored in the form of a KD - Tree to obtain its shortest distance from the template point cloud data d i ;

[0044] Store the shortest distance d i in the x - direction corresponding to the shortest distance as an x - direction shortest distance array {dx i } Store the shortest distance d iThe shortest distance in the corresponding y direction is stored as an array of the shortest distances in the y direction {dy i } ;

[0045] The step S22 includes:

[0046] Based on the array of the shortest distances in the x direction {dx i } and the array of the shortest distances in the y direction {dy i } , update the displacement estimation value, and the update relationship includes:

[0047] ;

[0048] ;

[0049] Among them, med() represents the median of the array, (Δx, Δy) represents the current displacement estimation value, (Δx’, Δy’) represents the new displacement estimation value;

[0050] The calculation formula of the step S23 includes:

[0051] ;

[0052] Among them, D represents the offset;

[0053] The update relationship for updating the target point cloud data based on the displacement estimation value in the step S24 includes:

[0054] ;

[0055] ;

[0056] Among them, represents the current target point cloud data; represents the new target point cloud data.

[0057] Optionally, the step S3 includes: Based on the displacement estimation value (Δx, Δy) and the initial position of the robot (x 0 ,y 0 ) , calculate and output the current position of the robot (x, y) , and the calculation formula includes:

[0058] ;

[0059] ;

[0060] After step S3, it further includes:

[0061] Compare whether the displacement estimation value is less than a preset displacement estimation threshold. If so, return to step S11; if not, update the template point cloud data and update the initial position of the robot, and then return to step S11;

[0062] The update of the template point cloud data includes: storing the target point cloud data after data preprocessing in step S12 as the new template point cloud data in the form of a KD - Tree;

[0063] The update of the initial position of the robot includes: taking the current position of the robot in step S3 as the new initial position.

[0064] A positioning device based on point cloud data, comprising:

[0065] An acquisition module for acquiring a circle of point cloud data as target point cloud data;

[0066] A displacement estimation module for obtaining the shortest distance between each coordinate point in the target point cloud data and the template point cloud data, and calculating the displacement estimation value based on the shortest distance;

[0067] A position calculation module for calculating the current position of the robot based on the displacement estimation value and the initial position of the robot.

[0068] Optionally, the acquisition module includes:

[0069] A template acquisition unit for acquiring a circle of point cloud data as template point cloud data; when acquiring a circle of point cloud data as template point cloud data, the robot is in a stationary state;

[0070] A target acquisition unit for acquiring a circle of point cloud data as target point cloud data;

[0071] The template acquisition unit is executed before the target acquisition unit;

[0072] The positioning device further includes:

[0073] A data preprocessing module for preprocessing the acquired point cloud data;

[0074] A template storage module for storing the template point cloud data after data preprocessing in the form of a KD - Tree;

[0075] A position initialization module for the initial position of the robot (x 0 ,y 0 )Initialized as (0,0) ;

[0076] The displacement estimation module includes:

[0077] A retrieval unit for obtaining the shortest distance between each coordinate point in the target point cloud data and the template point cloud data;

[0078] An estimation calculation unit for calculating the displacement estimation based on the shortest distance;

[0079] An offset calculation unit for calculating the offset based on the displacement estimation;

[0080] An offset comparison unit for comparing whether the offset is less than a preset offset threshold. If so, execute the position calculation module. If not, update the target point cloud data based on the displacement estimation, and then return to the retrieval unit to continue execution.

[0081] Optionally, the data format of the point cloud data is (r, θ + φ) , where r is the distance value collected by the lidar, θ is the lidar scanning angle value, φ is the self-rotation angle value collected by the attitude sensor;

[0082] The data preprocessing module includes:

[0083] A screening unit for screening the collected point cloud data to remove r < 3 or r = 0 coordinate points;

[0084] A coordinate conversion unit for performing coordinate conversion on the screened point cloud data, converting the r , θ and φ in polar coordinates to x and y in rectangular coordinates. The conversion relationship includes:

[0085] ;

[0086] ;

[0087] where the subscript i represents the i th coordinate point in the point cloud data, N represents the total number of coordinate points in the point cloud data;

[0088] A de-centralization unit for performing de-centralization processing on the point cloud data after coordinate conversion. The calculation formula includes:

[0089] ;

[0090] ;

[0091] ;

[0092] ;

[0093] where the subscript i represents the i th coordinate point in the point cloud data, N and

[0094] the data format of the point cloud data after the data preprocessing is a two-dimensional array (x ci ,y ci ) ; the template point cloud data is identified by the superscript 0 and the target point cloud data is identified by the superscript 1 ;

[0095] The displacement estimation module further includes an estimation initialization unit that is executed before the retrieval unit, and the estimation initialization unit is used to initialize the displacement estimation (Δx, Δy) with the calculation formula including:

[0096] ;

[0097] ;

[0098] The retrieval unit is specifically configured to retrieve each coordinate point in the target point cloud data in the template point cloud data stored in the form of a KD - Tree to obtain its shortest distance from the template point cloud data d i ; store the shortest distance d i corresponding to the x - direction as the x - direction shortest distance array {dx i } and store the shortest distance d i corresponding to the y - direction as the y - direction shortest distance array {dy i } ;

[0099] The valuation calculation unit is specifically configured to be based on the x - direction shortest distance array {dx i } and the y - direction shortest distance array {dy i} , update the displacement estimation value, and the update relationship includes:

[0100] ;

[0101] ;

[0102] Among them, med() represents the median of the array, (Δx, Δy) represents the current displacement estimation value, (Δx’, Δy’) represents the new displacement estimation value;

[0103] The calculation formula of the said offset calculation unit includes:

[0104] ;

[0105] Among them, D represents the offset;

[0106] The update relationship for updating the target point cloud data based on the displacement estimation value in the said offset comparison unit includes:

[0107] ;

[0108] ;

[0109] Among them, represents the current target point cloud data; represents the new target point cloud data;

[0110] The calculation formula of the said position calculation module includes:

[0111] ;

[0112] ;

[0113] Among them, (x, y) represents the current position of the robot;

[0114] The said positioning device further includes an estimation comparison module, which is used to compare whether the displacement estimation value is less than a preset displacement estimation threshold. If so, return to the target acquisition unit to continue execution; if not, update the template point cloud data and update the initial position of the robot, and then return to the target acquisition unit to continue execution;

[0115] The said update of the template point cloud data includes: storing the target point cloud data after data pre - processing as the new template point cloud data in the form of a KD - Tree;

[0116] The said update of the initial position of the robot includes: taking the current position of the robot as the new initial position;

[0117] The positioning device includes a lidar, an attitude sensor, and a single-chip microcomputer. The single-chip microcomputer includes an acquisition module, a displacement estimation module, a position calculation module, a data preprocessing module, a template storage module, a position initialization module, and an estimation comparison module. The lidar is electrically connected to the single-chip microcomputer, and the lidar transmits the distance values and scanning angle values it acquires to the single-chip microcomputer. The attitude sensor is electrically connected to the single-chip microcomputer, and the attitude sensor transmits the self-rotation angle values it acquires to the single-chip microcomputer.

[0118] A computer-readable storage medium stores computer instructions thereon. When the computer instructions run on a processor of an electronic device, the electronic device is caused to execute the method as described above.

[0119] Compared with the prior art, the present invention has the following beneficial effects:

[0120] The positioning method of the present invention calculates the displacement estimation using point cloud data and then calculates the current position of the robot based on the displacement estimation. During the process, it is not necessary to use the data collected by GPS and vision sensors. Therefore, it can be applied in a "double-blind" scenario. Moreover, this method does not need to use the point cloud map commonly used in existing positioning technologies. The point cloud map is the fusion of multiple frames of point cloud data, while this method only needs to use a single frame of point cloud data to achieve positioning. Therefore, the amount of calculated data is smaller than that of existing positioning technologies and can run on low-cost platforms with low computing power such as single-chip microcomputers. BRIEF DESCRIPTION OF THE DRAWINGS

[0121] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0122] Figure 1 It is a flowchart of the positioning method provided by an embodiment of the present invention;

[0123] Figure 2 It is another flowchart of the positioning method provided by an embodiment of the present invention;

[0124] Figure 3 It is a point cloud map of the displacement estimation iterative calculation once provided by an embodiment of the present invention;

[0125] Figure 4 It is a point cloud map of the displacement estimation iterative calculation three times provided by an embodiment of the present invention;

[0126] Figure 5The point cloud map for 24 - time iterative calculation of displacement estimation provided by the embodiment of the present invention;

[0127] Figure 6 The numerical graph for iterative calculations of different numbers of times of displacement estimation provided by the embodiment of the present invention;

[0128] Figure 7 The structural schematic diagram of the positioning device provided by the embodiment of the present invention.

[0129] Illustration: 1. Acquisition module; 11. Template acquisition unit; 12. Target acquisition unit; 2. Displacement estimation module; 20. Estimation initialization unit; 21. Retrieval unit; 22. Estimation calculation unit; 23. Offset calculation unit; 24. Offset comparison unit; 3. Position calculation module; 4. Estimation comparison module; 5. Position initialization module; 6. Data pre - processing module; 7. Template storage module. Detailed implementation manners

[0130] To make the invention purpose, features, and advantages of the present invention more obvious and understandable, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the embodiments described below are only a part of the embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0131] The technical solutions of the present invention will be further described below with reference to the accompanying drawings and through specific implementation manners.

[0132] The present invention provides a positioning method based on point cloud data. Please refer to Figure 1 , and this positioning method includes the following steps:

[0133] S1. Collect a circle of point cloud data as target point cloud data.

[0134] S2. Obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data, and calculate the displacement estimation based on the shortest distance.

[0135] S3. Calculate the current position of the robot based on the displacement estimation and the initial position of the robot.

[0136] The positioning method provided in this embodiment calculates the displacement estimate using point cloud data and then calculates the current position of the robot based on the displacement estimate to achieve positioning. During the whole process, the data collected by GPS and vision sensors are not required, so it can be applied in the "double-blind" scenario. Moreover, this method does not need to use the point cloud map commonly used in existing positioning technologies. The point cloud map is the fusion of multiple frames of point cloud data, while this method only needs to use a single frame of point cloud data to achieve positioning. Therefore, the amount of calculated data is smaller than that of existing positioning technologies and can run on low-cost platforms with low computing power such as single-chip microcomputers.

[0137] Please refer to Figure 2 , in some embodiments, before step S1, there are also steps of setting the initial position of the robot and obtaining the template point cloud data, specifically including:

[0138] S01. Initialize the initial position of the robot (x 0 ,y 0 ) to (0,0) .

[0139] S02. Collect a circle of point cloud data as the template point cloud data.

[0140] Specifically, when executing step S02, the robot is in a stationary state, that is, the initial position of the robot at this time (x 0 ,y 0 ) is (0,0) .

[0141] S03. Perform data preprocessing on the collected template point cloud data.

[0142] S04. Store the template point cloud data after data preprocessing in the form of a KD - Tree.

[0143] It can be understood that steps S02 - 04 should be executed in sequence, while steps S01 and steps S02 - 04 do not necessarily need to be executed in sequence. Whether step S01 is before S02 or after S04 has no impact on the result of this method. Simply changing the order of steps should still be within the protection scope of the present invention.

[0144] In this embodiment, similar to performing data preprocessing on the collected template point cloud data, the target point cloud data collected in step S1 also needs to be preprocessed, that is, step S1 specifically includes:

[0145] S11. Collect a circle of point cloud data as the target point cloud data.

[0146] Specifically, when performing step S11, the robot can be in a moving state or a stationary state.

[0147] S12. Perform data preprocessing on the collected target point cloud data.

[0148] In some embodiments, the data formats of the point cloud data collected in step S03 and step S12 are both (r, θ + φ) , where r is the distance value collected by the lidar, θ is the lidar scanning angle value, φ is the self-rotation angle value (i.e., yaw angle) collected by the attitude sensor. Here, the attitude sensor refers to various sensors that can obtain the yaw angle, such as an inertial measurement unit (IMU) and an odometer, etc. When collecting the point cloud data, the positioning method provided in this embodiment utilizes both the lidar and the attitude sensor at the same time. Both of them can work in a "double-blind" environment without GPS signal and light. And using the data of both at the same time will result in higher positioning accuracy compared to only using the data of one of them.

[0149] In some embodiments, the data preprocessing processes in step S03 and step S12 are the same, only the processing objects are different. The data preprocessing specifically includes the following steps:

[0150] Screen the collected point cloud data to remove r < 3 or r = 0 coordinate points;

[0151] Perform coordinate transformation on the screened point cloud data, and transform the r , θ and φ in polar coordinates into x and y in rectangular coordinates. The transformation relationships include:

[0152] ;

[0153] ;

[0154] where the subscript i represents the i rd coordinate point in the point cloud data, N represents the total number of coordinate points in the point cloud data;

[0155] Perform decentralization processing on the point cloud data after coordinate transformation. The calculation formulas include:

[0156] ;

[0157] ;

[0158] ;

[0159] ;

[0160] where the subscript i represents the i th coordinate point in the point cloud data, N represents the total number of coordinate points in the point cloud data.

[0161] The data format of the point cloud data after the above data preprocessing is a two-dimensional array (x ci ,y ci ) , in the above formula i= 1...N represents i and its value range is 1 to N . If the object of data preprocessing is the template point cloud data, it is identified by the superscript 0 in the above formula. If the object of data preprocessing is the template point cloud data, it is identified by the superscript 1 in the above formula.

[0162] In the above step of screening the collected point cloud data, r coordinate points less than 3 indicate that the object is too close to the lidar; r coordinate points equal to 0 indicate that the object is too far from the lidar, exceeding the scanning range of the radar. Removing these coordinate points can avoid the deviation caused by these points to improve the positioning accuracy. And the de-centering process can translate the point cloud data to the origin as the center for convenient subsequent calculation.

[0163] From the above data preprocessing process, it can be seen that step S03 should specifically include:

[0164] S031. Screen the template point cloud data and remove r coordinate points less than 3 or r equal to 0.

[0165] S032. Perform coordinate transformation on the screened template point cloud data, and convert r , θ and φ in polar coordinates to x and y in rectangular coordinates. The conversion relationship includes:

[0166] ;

[0167] ;

[0168] where the subscript i represents the i th coordinate point in the point cloud data, N represents the total number of coordinate points in the point cloud data.

[0169] S033. Decentralize the template point cloud data after coordinate transformation. The calculation formulas include:

[0170] ;

[0171] ;

[0172] ;

[0173] ;

[0174] where the subscript i represents the i th coordinate point in the point cloud data, N represents the total number of coordinate points in the point cloud data. The data format of the template point cloud data after data preprocessing is a two-dimensional array .

[0175] Similarly, step S12 should specifically include:

[0176] S121. Filter the target point cloud data to remove r < 3 or r = 0 coordinate points.

[0177] S122. Perform coordinate transformation on the filtered target point cloud data, and convert r , θ and φ in polar coordinates to x and y in rectangular coordinates. The conversion relationships include:

[0178] ;

[0179] ;

[0180] S123. Decentralize the target point cloud data after coordinate transformation. The calculation formulas include:

[0181] ;

[0182] ;

[0183] ;

[0184] ;

[0185] Among them, the subscript i represents the i th coordinate point in the point cloud data, N represents the total number of coordinate points in the point cloud data. The data format of the target point cloud data after data preprocessing is a two-dimensional array .

[0186] In the above step S04, KD - Tree is short for K - Dimensional Tree, which is a data structure for partitioning k - dimensional data space and can be applied to the search for key data in multi - dimensional space. KD - Tree can effectively improve the search efficiency and reduce the requirements for hardware performance of the positioning method.

[0187] The basic construction process of KD - Tree is as follows:

[0188] Select the splitting dimension: Select the dimension with the largest variance from all dimensions as the splitting dimension (or preset an initial splitting dimension by yourself);

[0189] Select the median: Select the median of the coordinate points in this dimension as the splitting point;

[0190] Split the data: Divide the coordinate points into two parts according to the splitting dimension, and recursively construct the left and right sub - trees respectively;

[0191] Recursive construction: Repeat the above process for the left and right sub - trees until each leaf node contains only one coordinate point.

[0192] In some embodiments, step S2 includes:

[0193] S21. Obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data.

[0194] S22. Calculate the displacement estimate based on the shortest distance.

[0195] S23. Calculate the offset based on the displacement estimate.

[0196] S24. Compare whether the offset is less than the preset offset threshold. If so, execute step S3; if not, update the target point cloud data based on the displacement estimate, and then return to step S21.

[0197] Calculating the displacement estimate only once will result in a large error. By introducing the concept of offset for iterative calculation of the displacement estimate, the calculation result of the displacement estimate can be made more accurate. A more accurate displacement estimate can make the calculation result of the current position of the robot more accurate, thereby obtaining higher positioning accuracy.

[0198] Furthermore, step S2 specifically includes:

[0199] S20. Initialize the displacement estimation, and the calculation formula includes: (Δx, Δy) ;

[0200] ;

[0201] .

[0202] S21. Retrieve each coordinate point in the target point cloud data from the template point cloud data stored in the form of a KD - Tree to obtain its shortest distance from the template point cloud data d i ;

[0203] Store the shortest distance d i corresponding to the x - direction as the x - direction shortest distance array {dx i } , and store the shortest distance d i corresponding to the y - direction as the y - direction shortest distance array {dy i } .

[0204] Among them, the shortest distance d i adopts the Manhattan distance, that is d i = dx i + dy i . The calculation amount of the Manhattan distance is relatively small, which can reduce the requirements of the positioning method for hardware performance.

[0205] S22. Based on the x - direction shortest distance array {dx i } and the y - direction shortest distance array {dy i } , update the displacement estimation, and the update relationship includes:

[0206] ;

[0207] ;

[0208] Among them, med() represents the median of the array, (Δx, Δy) represents the current displacement estimation, (Δx’, Δy’) represents the new displacement estimation.

[0209] In this embodiment, the displacement estimation is calculated based on the median of the array. Taking the entire array as a reference can reduce the deviation caused by only referring to a small amount of data. Moreover, the median is selected, which can reduce the influence of extreme values compared with the average value and improve the accuracy of the result. In addition, since our method uses the median as the displacement estimation, it does not require too much point cloud data to reflect the displacement of the robot, that is, a high-resolution radar is not required.

[0210] It can be understood that the new displacement estimation is the current displacement estimation in the subsequent steps. That is to say, the displacement estimation in the steps executed after step S22 is the new displacement estimation, and iteration is continuously performed in this way. For example, in the same round of iteration, the displacement estimation in step S23 should be the new displacement estimation in step S22; the current displacement estimation in step S22 in the next round of iteration should be the new displacement estimation in step S22 in the previous round of iteration.

[0211] S23. Calculate the offset based on the displacement estimation. The calculation formula includes:

[0212] ;

[0213] Among them, D represents the offset.

[0214] S24. Compare whether the offset is less than the preset offset threshold. If so, execute step S3; if not, update the target point cloud data based on the displacement estimation, and then return to step S21.

[0215] The update relationship for updating the target point cloud data based on the displacement estimation includes:

[0216] ;

[0217] ;

[0218] Among them, represents the current target point cloud data; represents the new target point cloud data.

[0219] Similarly, the new target point cloud data is the current target point cloud data in the subsequent steps, which will not be elaborated here.

[0220] As mentioned above, the error will be relatively large if the displacement estimation is only calculated once. By introducing the concept of offset and performing iterative calculation on the displacement estimation, the calculation can be made more accurate. The difference between the iterative calculation of the displacement estimation and the non-iterative calculation of the displacement estimation only once is as Figures 3 - 5 shown, Figure 3 is the point cloud diagram of the displacement estimation calculated iteratively once, Figure 4 is the point cloud diagram of the displacement estimation calculated iteratively three times, Figure 5It is a point cloud map for iteratively calculating the displacement estimation 24 times. Figures 3 - 5 Among them, the point cloud map represented by multiple points "..." is the point cloud data scanned before the robot moves, and the point cloud map represented by multiple circles "..." is the point cloud data scanned after the robot moves. The better the two point cloud maps overlap, the more accurate the displacement estimation is. From Figures 3 - 5 It can be seen that the accuracy of the displacement estimation for 24 iterations, 3 iterations, and 1 iteration decreases in turn. Figure 6 It is the numerical value of the displacement estimation obtained under different numbers of iterations. Among them, the upper figure is the Δx numerical value, and the lower figure is the Δy numerical value. It can be seen from the figure that as the number of iterations increases, the displacement estimation gradually approaches a fixed value, that is, the displacement estimation basically no longer changes after a limited number of iterations. At this time, it can be considered that the accuracy of the displacement estimation meets the requirements. In this embodiment, the offset is used to determine whether the displacement estimation is accurate enough. When the offset is less than the preset offset threshold, it is considered that the displacement estimation is accurate enough, and the loop ends at this time.

[0221] In this embodiment, step S3 specifically includes:

[0222] Based on the displacement estimation (Δx, Δy) and the initial position of the robot (x 0 ,y 0 ) calculate and output the current position of the robot (x, y) , and the calculation formula includes:

[0223] ;

[0224] .

[0225] In some embodiments, after step S3, it further includes:

[0226] S4. Compare whether the displacement estimation is less than the preset displacement estimation threshold. If so, return to step S11; if not, update the template point cloud data and update the initial position of the robot, and then return to step S11.

[0227] Among them, updating the template point cloud data includes: storing the target point cloud data after data preprocessing in step S12 as the new template point cloud data in the form of a KD - Tree;

[0228] Updating the initial position of the robot includes: taking the current position of the robot in step S3 as the new initial position.

[0229] After calculating the current position of the robot once, if it is necessary to continuously obtain the current position of the robot in real time, it is necessary to collect point cloud data again as the target point cloud data and continue to calculate it continuously in a loop. Before collecting the point cloud data again, if the displacement estimate is not less than the preset displacement estimate threshold, it is considered that the robot has moved a relatively long distance relative to its initial position. At this time, after updating the initial position of the robot and the template point cloud data, and then performing the next round of iteration, the positioning accuracy can be made more accurate.

[0230] In the present invention, the judgment criterion for the threshold is whether it is "less than". It can be understood that "less than" can also be changed to whether it is "less than or equal to" according to needs. Such simple changes should be within the protection scope of the present invention.

[0231] This embodiment also provides a positioning device based on point cloud data. Please refer to Figure 7 , the positioning device includes:

[0232] The acquisition module 1 is used to acquire a circle of point cloud data as the target point cloud data;

[0233] The displacement estimation module 2 is used to obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data, and calculate the displacement estimate based on the shortest distance;

[0234] The position calculation module 3 is used to calculate the current position of the robot based on the displacement estimate and the initial position of the robot.

[0235] In some embodiments, the acquisition module 1 includes:

[0236] The template acquisition unit 11 is used to acquire a circle of point cloud data as the template point cloud data; when acquiring a circle of point cloud data as the template point cloud data, the robot is in a stationary state;

[0237] The target acquisition unit 12 is used to acquire a circle of point cloud data as the target point cloud data.

[0238] Specifically, the template acquisition unit 11 is executed before the target acquisition unit 12.

[0239] In some embodiments, the positioning device further includes:

[0240] The position initialization module 5 is used to initialize the initial position of the robot (x 0 ,y 0 ) to (0,0) ;

[0241] The data preprocessing module 6 is used to perform data preprocessing on the collected point cloud data;

[0242] The template storage module 7 is used to store the template point cloud data after data preprocessing in the form of a KD - Tree.

[0243] In some embodiments, the displacement estimation module 2 includes:

[0244] The retrieval unit 21 is used to obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data;

[0245] The estimation calculation unit 22 is used to calculate the displacement estimation based on the shortest distance;

[0246] The offset calculation unit 23 is used to calculate the offset based on the displacement estimation;

[0247] The offset comparison unit 24 is used to compare whether the offset is less than a preset offset threshold. If so, the position calculation module 3 is executed. If not, the target point cloud data is updated based on the displacement estimation, and then it returns to the retrieval unit 21 to continue execution.

[0248] In some embodiments, the data preprocessing module 6 includes:

[0249] The screening unit is used to screen the collected point cloud data and remove r < 3 or r = 0 coordinate points;

[0250] The coordinate conversion unit is used to perform coordinate conversion on the screened point cloud data, and convert the r , θ and φ in polar coordinates to x and y in rectangular coordinates. The conversion relationship includes:

[0251] ;

[0252] ;

[0253] where the subscript i represents the i th coordinate point in the point cloud data, and N represents the total number of coordinate points in the point cloud data;

[0254] The de - centering unit performs de - centering processing on the point cloud data after coordinate conversion. The calculation formula includes:

[0255] ;

[0256] ;

[0257] ;

[0258] ;

[0259] Among them, the subscript i represents the i th coordinate point in the point cloud data, N and represents the total number of coordinate points in the point cloud data.

[0260] The data format of the point cloud data after data preprocessing is a two-dimensional array (x ci ,y ci ) ; The template point cloud data is identified by the superscript 0 , and the target point cloud data is identified by the superscript 1 .

[0261] In some embodiments, the displacement estimation module 2 further includes an estimation initialization unit 20 that is executed before the retrieval unit 21. The estimation initialization unit 20 is used to initialize the displacement estimation (Δx, Δy) , and the calculation formula includes:

[0262] ;

[0263] .

[0264] In some embodiments, the retrieval unit 21 is specifically configured to retrieve each coordinate point in the target point cloud data in the template point cloud data stored in the form of a KD - Tree to obtain its shortest distance from the template point cloud data d i ; Store the shortest distance d i corresponding to the x - direction as the x - direction shortest distance array {dx i } , and store the shortest distance d i corresponding to the y - direction as the y - direction shortest distance array {dy i } .

[0265] In some embodiments, the estimation calculation unit 22 is specifically configured to update the displacement estimation based on the x - direction shortest distance array {dx i } and the y - direction shortest distance array {dy i } , and the update relationship includes:

[0266] ;

[0267] ;

[0268] Among them, med() represents the median of the array, (Δx, Δy) represents the current displacement estimation value, (Δx’, Δy’) represents the new displacement estimation value.

[0269] In some embodiments, the calculation formula of the offset calculation unit 23 includes:

[0270] ;

[0271] Among them, D represents the offset.

[0272] In some embodiments, the update relationship for updating the target point cloud data based on the displacement estimation value in the offset comparison unit 24 includes:

[0273] ;

[0274] ;

[0275] Among them, represents the current target point cloud data; represents the new target point cloud data.

[0276] In some embodiments, the calculation formula of the position calculation module 3 includes:

[0277] ;

[0278] .

[0279] Among them, (x, y) represents the current position of the robot.

[0280] In some embodiments, the positioning device further includes an estimation comparison module 4, which is used to compare whether the displacement estimation value is less than a preset displacement estimation threshold. If so, it returns to the target acquisition unit 12 to continue execution; if not, it updates the template point cloud data and updates the initial position of the robot, and then returns to the target acquisition unit 12 to continue execution.

[0281] Among them, updating the template point cloud data includes: storing the target point cloud data after data preprocessing in the data preprocessing module 6 as the new template point cloud data in the form of a KD - Tree;

[0282] Updating the initial position of the robot includes: using the current position of the robot in the position calculation module 3 as the new initial position;

[0283] In some embodiments, the positioning device includes a lidar, an attitude sensor, and a single-chip microcomputer. The single-chip microcomputer includes an acquisition module 1, a displacement estimation module 2, a position calculation module 3, a data preprocessing module 6, a template storage module 7, a position initialization module 5, and an estimation comparison module 4. The lidar is electrically connected to the single-chip microcomputer, and the lidar transmits the distance values and scanning angle values it acquires to the single-chip microcomputer. The attitude sensor is electrically connected to the single-chip microcomputer, and the attitude sensor transmits the self-rotation angle values it acquires to the single-chip microcomputer.

[0284] All relevant content of each step involved in the foregoing embodiments of the positioning device can be cited to the corresponding description in the relevant content of the positioning method, and will not be further elaborated here.

[0285] In this embodiment, the functional modules of the positioning device are divided according to the above method examples. The functional modules can be implemented in the form of hardware or by hardware executing corresponding software. Whether a certain function is executed in the form of hardware or computer software driving hardware depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application in combination with the embodiments, but such implementation should not be considered to exceed the scope of this application.

[0286] It should be noted that the division of modules in this embodiment is illustrative, only a logical function division, and there may be other division methods in actual implementation. For example, each functional module can be divided corresponding to each function, or two or more functions can be integrated into one processing module.

[0287] This embodiment also provides a computer storage medium, in which computer instructions are stored. When the computer instructions run on an electronic device, the electronic device is enabled to execute the foregoing positioning method.

[0288] If the integrated functional unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a readable storage medium. Based on this understanding, the technical solution of the embodiments of this application, in essence, or the part that contributes to the prior art, or all or part of this technical solution, can be embodied in the form of a software product. The software product is stored in a storage medium and includes several instructions to enable a device (which can be a single-chip microcomputer, a chip, etc.) or a processor to execute all or part of the steps of the methods of the various embodiments of this application. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read only memory (ROM), random access memory (RAM), magnetic disks, or optical discs that can store program codes.

[0289] The positioning device and computer storage medium provided by the present invention are both used to execute the corresponding methods provided above. Therefore, the beneficial effects that can be achieved can refer to the beneficial effects in the corresponding methods provided above, and will not be elaborated here.

[0290] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning method based on point cloud data, characterized in that: The following steps are involved: S1, collect a circle of point cloud data as target point cloud data; S2, obtaining the shortest distance between each coordinate point in the target point cloud data and the template point cloud data, and calculating the displacement estimation based on the shortest distance; S20, estimate the displacement (Δx,Δy) initialization; S21, retrieve each coordinate point in the target point cloud data from the template point cloud data stored in the form of KD-Tree to obtain the shortest distance between it and the template point cloud data d i ; The shortest distance d i The corresponding shortest distance in the x direction is stored as the shortest distance array in the x direction {dx i } , the shortest distance d i The corresponding shortest distance in the y direction is stored as the shortest distance array in the y direction {dy i } ; S22, based on the shortest distance array in the x direction {dx i } and the shortest distance array in the y direction {dy i } , update the displacement estimate; S23, calculating an offset based on the displacement estimation; S24, comparing whether the offset is less than a preset offset threshold, if so, executing step S3; if not, updating the target point cloud data based on the displacement estimation, and then returning to step S21; S3. Calculate the current position of the robot based on the displacement estimation and the initial position of the robot.

2. The positioning method based on point cloud data according to claim 1, characterized in that: Before step S1, the following steps are also included: Collect a circle of point cloud data as template point cloud data; Perform data preprocessing on the collected template point cloud data; The template point cloud data after data preprocessing is stored in the form of KD-Tree; The step S1 comprises: S11, collecting a circle of point cloud data as target point cloud data; S12, performing data preprocessing on the collected target point cloud data; Before step S1, the following steps are also included: Set the robot's initial position (x 0 ,y 0 ) Initialize to (0,0) ; When collecting a circle of point cloud data as template point cloud data, the robot is in a stationary state.

3. The positioning method based on point cloud data according to claim 2, characterized in that: The data format of the point cloud data is (r,θ+φ) ,in, r is the distance value collected by the laser radar, θ is the laser radar scanning angle value, φ The rotation angle value collected by the attitude sensor; The data preprocessing includes: Filter the collected point cloud data and remove r <3 or r =0 coordinate point; The filtered point cloud data is converted into coordinates. r , θ and φ Convert to rectangular coordinates x and y , the conversion relationships include: ； ； Among them, the subscript i Represents the first i coordinate points, N Indicates the total number of coordinate points in the point cloud data; The point cloud data after coordinate conversion is decentralized, and the calculation formula includes: ； ； ； ； Among them, the subscript i Represents the first i coordinate points, N Indicates the total number of coordinate points in the point cloud data; The data format of the point cloud data after data preprocessing is a two-dimensional array (x ci ,y ci ) ; The template point cloud data is superscript 0 Identification, the target point cloud data is superscript 1 Logo.

4. The positioning method based on point cloud data according to claim 3, characterized in that: The displacement estimation in step S20 (Δx,Δy) The initialization calculation formula includes: ； ； The updating relationship of updating the displacement estimation in step S22 includes: ； ； in, med() Represents the median of an array. (Δx,Δy) represents the current displacement estimate, (Δx',Δy') represents the new displacement estimate; The calculation formula for calculating the offset in step S23 includes: ； in, D Indicates the offset; The updating relationship of updating the target point cloud data in step S24 includes: ； ； in, Represents the current target point cloud data; Represents the new target point cloud data.

5. The positioning method based on point cloud data according to claim 2, characterized in that: The step S3 comprises: based on the displacement estimation (Δx,Δy) and the robot's initial position (x 0 ,y 0 ) , calculate and output the current position of the robot (x,y) , the calculation formula includes: ； ； After step S3, the following steps are also included: Compare whether the displacement estimate is less than a preset displacement estimate threshold, if so, return to step S11; if not, update the template point cloud data and the initial position of the robot, and then return to step S11; The updating of the template point cloud data includes: storing the target point cloud data after the data preprocessing in step S12 as new template point cloud data in the form of KD-Tree; Updating the initial position of the robot includes: taking the current position of the robot in step S3 as a new initial position.

6. A positioning device based on point cloud data, characterized in that: include: A collection module is used to collect a circle of point cloud data as target point cloud data; A displacement estimation module is used to obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data, and calculate the displacement estimation based on the shortest distance; A position calculation module, used to calculate the current position of the robot based on the displacement estimation and the initial position of the robot; The displacement estimation module comprises: An estimation initialization unit is used to estimate the displacement (Δx,Δy) initialization; The retrieval unit is used to retrieve each coordinate point in the target point cloud data from the template point cloud data stored in the form of KD-Tree to obtain the shortest distance between it and the template point cloud data. d i ; The shortest distance d i The corresponding shortest distance in the x direction is stored as the shortest distance array in the x direction {dx i } , the shortest distance d i The corresponding shortest distance in the y direction is stored as the shortest distance array in the y direction {dy i } ; Estimation calculation unit for the shortest distance array in the x direction {dx i } and the shortest distance array in the y direction {dy i } , update the displacement estimate; An offset calculation unit, configured to calculate an offset based on the displacement estimate; The offset comparison unit is used to compare whether the offset is less than a preset offset threshold. If so, the position calculation module is executed. If not, the target point cloud data is updated based on the displacement estimation, and then the retrieval unit is returned to continue execution.

7. The positioning device based on point cloud data according to claim 6, characterized in that: The acquisition module comprises: The template collection unit is used to collect a circle of point cloud data as template point cloud data; when collecting a circle of point cloud data as template point cloud data, the robot is in a stationary state; A target acquisition unit, used for acquiring a circle of point cloud data as target point cloud data; The template acquisition unit is executed before the target acquisition unit; The positioning device also includes: A data preprocessing module is used to preprocess the collected point cloud data; The template storage module is used to store the template point cloud data after data preprocessing in the form of KD-Tree; Position initialization module, used to initialize the robot's initial position (x 0 ,y 0 ) Initialize to (0,0)。 8. The positioning device based on point cloud data according to claim 7, characterized in that: The data format of the point cloud data is (r,θ+φ) ,in, r is the distance value collected by the laser radar, θ is the laser radar scanning angle value, φ The rotation angle value collected by the attitude sensor; The data preprocessing module comprises: The screening unit is used to screen the collected point cloud data and remove r <3 or r =0 coordinate point; The coordinate conversion unit is used to convert the filtered point cloud data into r , θ and φ Convert to rectangular coordinates x and y , the conversion relationships include: ； ； Among them, the subscript i Represents the first i coordinate points, N Indicates the total number of coordinate points in the point cloud data; The decentralization unit is used to decentralize the point cloud data after coordinate transformation. The calculation formula includes: ； ； ； ； Among them, the subscript i Represents the first i coordinate points, N Indicates the total number of coordinate points in the point cloud data; The data format of the point cloud data after data preprocessing is a two-dimensional array (x ci ,y ci ) ; The template point cloud data is superscript 0 Identification, the target point cloud data is superscript 1 Logo.

9. The positioning device based on point cloud data according to claim 7, characterized in that: The displacement estimation in the estimation initialization unit (Δx,Δy) The initialization calculation formula includes: ； ； The updating relationship of updating the displacement estimation in the estimation calculation unit includes: ； ； in, med() Represents the median of an array. (Δx,Δy) represents the current displacement estimate, (Δx',Δy') represents the new displacement estimate; The calculation formula for calculating the offset in the offset calculation unit includes: ； in, D Indicates the offset; The updating relationship of updating the target point cloud data in the offset comparison unit includes: ； ； in, Represents the current target point cloud data; Represents new target point cloud data; The calculation formula of the position calculation module includes: ； ； in, (x,y) Indicates the current position of the robot; The positioning device also includes an estimation comparison module, which is used to compare whether the displacement estimation is less than a preset displacement estimation threshold value, and if so, returns to the target acquisition unit to continue execution; if not, updates the template point cloud data and the initial position of the robot, and then returns to the target acquisition unit to continue execution; The updating of the template point cloud data includes: storing the target point cloud data after data preprocessing as new template point cloud data in the form of KD-Tree; Updating the initial position of the robot includes: taking the current position of the robot as a new initial position; The positioning device includes a laser radar, a posture sensor and a single-chip microcomputer, and the single-chip microcomputer includes the acquisition module, the displacement estimation module, the position calculation module, the data preprocessing module, the template storage module, the position initialization module and the estimation comparison module; the laser radar is electrically connected to the single-chip microcomputer, and the laser radar transmits the distance value and scanning angle value collected by it to the single-chip microcomputer; the posture sensor is electrically connected to the single-chip microcomputer, and the posture sensor transmits the rotation angle value collected by it to the single-chip microcomputer.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and when the computer instructions are executed on a processor of an electronic device, the electronic device executes the method according to any one of claims 1 to 5.

Images