Positioning method and device based on point cloud data

Abstract

The invention discloses a positioning method and device based on point cloud data. The method comprises 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 estimated value based on the shortest distance; and S3, calculating the current position of the robot based on the displacement estimated value and the initial position of the robot. According to the positioning method, the displacement estimated value is obtained through calculation of the point cloud data to achieve positioning, data collected by a GPS and a visual sensor do not need to be used in the process, and therefore the method can be applied to a double-blind scene. According to the method, a point cloud map conventionally used in an existing positioning technology does not need to be utilized, the point cloud map is fusion of multiple frames of point cloud data, and positioning can be achieved only by utilizing single-frame point cloud data, so that the calculated data volume is smaller compared with that of the existing positioning technology, and the method can be operated on low-cost platforms such as a single-chip microcomputer with low computing power.

Description

Technical Field

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

[0002] In recent years, micro-UAVs have developed rapidly and are widely used in real life due to their flexibility, strong maneuverability and low cost. The navigation method used by UAVs for autonomous flight is usually to estimate the navigation status by integrating GPS, visual sensors, lidar and inertial sensors. With the expansion of the scope of application, the demand for the application of UAVs in "double-blind" environments without GPS and light is gradually increasing. The positioning methods of existing technologies generally require the use of GPS and visual navigation, but GPS and visual navigation methods will fail in "double-blind" environments. Therefore, a positioning method that does not require GPS and visual sensors is urgently needed. 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 light.

[0004] To achieve this object, the present invention adopts the following technical solutions: A positioning method based on point cloud data comprises the following steps: 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; S3. Calculate the current position of the robot based on the displacement estimation and the initial position of the robot.

[0005] Optionally, step S2 includes: S21, obtaining the shortest distance between each coordinate point in the target point cloud data and the template point cloud data; S22, calculating displacement estimation based on the shortest distance; 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.

[0006] Optionally, before step S1, the step further includes: 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.

[0007] Optionally, the data format of the point cloud data is (r,θ+φ) ,in, r is the distance value collected by the laser radar, i is the laser radar scanning angle value, f 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 , i and f 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.

[0008] Optionally, before step S21, the step further includes: S20, estimate the displacement (Δx,Δy) Initialization, the calculation formula includes: ; ; The step S21 comprises: Each coordinate point in the target point cloud data is retrieved in 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 } ; The step S22 comprises: 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, and the updated relationship 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 of step S23 includes: ; in, D Indicates the offset; The updating relationship of updating the target point cloud data based on the displacement estimation in step S24 includes: ; ; in, Represents the current target point cloud data; Represents the new target point cloud data.

[0009] Optionally, step S3 includes: based on 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 update 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; The updating of the initial position of the robot includes: taking the current position of the robot in step S3 as a new initial position.

[0010] A positioning device based on point cloud data, comprising: 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; The position calculation module is used to calculate the current position of the robot based on the displacement estimation and the initial position of the robot.

[0011] Optionally, the acquisition module includes: 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) ; The displacement estimation module comprises: A retrieval unit, used to obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data; An estimation calculation unit, used for calculating a displacement estimation based on a shortest distance; 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.

[0012] Optionally, the data format of the point cloud data is (r,θ+φ) ,in, r is the distance value collected by the laser radar, i is the laser radar scanning angle value, f 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 , i and f 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; The displacement estimation module further includes an estimation initialization unit executed before the retrieval unit, wherein the estimation initialization unit is used to initialize the displacement estimation (Δx,Δy) Initialization, the calculation formula includes: ; ; The retrieval unit is specifically used to retrieve each coordinate point in the target point cloud data in the template point cloud data stored in the form of KD-Tree to obtain the shortest distance between the coordinate point 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 } ; The estimation calculation unit is specifically used to calculate 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, and the updated relationship 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 of the offset calculation unit includes: ; in, D Indicates the offset; The updating relationship of updating the target point cloud data based on the displacement estimation 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 updates 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; The updating of 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.

[0013] A computer-readable storage medium stores computer instructions. When the computer instructions are executed on a processor of an electronic device, the electronic device executes the method described above.

[0014] Compared with the prior art, the present invention has the following beneficial effects: The positioning method of the present invention uses point cloud data to calculate displacement estimation, and then obtains the current position of the robot based on the displacement estimation. In the process, there is no need to use the data collected by GPS and visual sensors, so it can be applied in "double-blind" scenarios. In addition, this method does not need to use the point cloud map commonly used in existing positioning technology. The point cloud map is a 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 technology, and it can be run on low-cost platforms with low computing power such as single-chip microcomputers. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0016] Figure 1 A flowchart of a positioning method provided by an embodiment of the present invention; Figure 2 Another flow chart of the positioning method provided by an embodiment of the present invention; Figure 3 A point cloud image of displacement estimation iteratively calculated once provided in an embodiment of the present invention; Figure 4 A point cloud image of displacement estimation iteratively calculated three times according to an embodiment of the present invention; Figure 5 A point cloud image of displacement estimation iteratively calculated 24 times provided in an embodiment of the present invention; Figure 6 A numerical diagram of displacement estimation iterative calculations of different times provided by an embodiment of the present invention; Figure 7 A schematic diagram of the structure of a positioning device provided in an embodiment of the present invention.

[0017] 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 preprocessing module; 7. Template storage module. DETAILED DESCRIPTION

[0018] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described below are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0019] The technical solution of the present invention is further described below with reference to the accompanying drawings and through specific implementation methods.

[0020] The present invention provides a positioning method based on point cloud data, please refer to Figure 1 , the positioning method comprises the following steps: S1. Collect a circle of point cloud data as target point cloud data.

[0021] 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 estimate based on the shortest distance.

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

[0023] The positioning method provided in this embodiment uses point cloud data to calculate displacement estimation and calculates the current position of the robot based on the displacement estimation in one step to achieve positioning. The data collected by GPS and visual sensors does not need to be used in the whole process, so it can be applied in "double-blind" scenarios. In addition, this method does not need to use the point cloud map commonly used in existing positioning technology. The point cloud map is a 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 technology, and it can be run on low-cost platforms with low computing power such as single-chip microcomputers.

[0024] Please refer to Figure 2 In some embodiments, before step S1, the steps of setting the initial position of the robot and obtaining template point cloud data are also included, which specifically include: S01, the initial position of the robot (x 0 ,y 0 ) Initialize to (0,0) .

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

[0026] 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 ) for (0,0) .

[0027] S03. Preprocessing the collected template point cloud data.

[0028] S04. The template point cloud data after data preprocessing is stored in the form of KD-Tree.

[0029] It is understandable that steps S02-04 should be executed in sequence, but step S01 and step S02-04 are not necessarily executed in sequence. Step S01 before S02 and after S04 has no effect on the result of this method. Simply changing the order of the steps should still be within the scope of protection of the present invention.

[0030] In this embodiment, similar to the data preprocessing performed 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: S11. Collect a circle of point cloud data as target point cloud data.

[0031] Specifically, when executing step S11, the robot may be in a moving state or in a stationary state.

[0032] S12. Preprocess the collected target point cloud data.

[0033] In some embodiments, the data format of the point cloud data collected in the above steps S03 and S12 is (r,θ +φ) ,in, r is the distance value collected by the laser radar, i is the laser radar scanning angle value, f The rotation angle value (i.e., yaw angle) collected by the attitude sensor. Among them, the attitude sensor refers to various sensors that can obtain the yaw angle, such as inertial measurement unit (IMU) and odometer. When collecting point cloud data, the positioning method provided in this embodiment uses both lidar and attitude sensor at the same time, both of which can work in a "double-blind" environment without GPS signal and light, and using the data of both at the same time will have higher positioning accuracy than using only one of them.

[0034] In some embodiments, the data preprocessing process in the above step S03 and step S12 is the same, only the processing objects are different, and the data preprocessing specifically includes the following steps: 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 , i and f 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.

[0035] 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 express i The value range is 1 to N If the object of data preprocessing is template point cloud data, then in the above formula, the superscript 0 If the object of data preprocessing is template point cloud data, then in the above formula, the superscript 1 Logo.

[0036] In the above steps of screening the collected point cloud data, r Coordinate points with a value less than 3 indicate that the object is too close to the LiDAR; r = 0 indicates that the object is too far away from the laser radar and exceeds the scanning range of the radar. Removing these coordinate points can avoid the deviation caused by these points to improve positioning accuracy. Decentralization processing can translate the point cloud data to the origin as the center, which is convenient for subsequent calculations.

[0037] It can be seen from the above data preprocessing process that step S03 should specifically include: S031. Filter the template point cloud data and remove r <3 or r =0 coordinate point.

[0038] S032, transform the filtered template point cloud data into coordinates, and convert the polar coordinates into r , i and f 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.

[0039] S033. Decentralize the template point cloud data after coordinate conversion. 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 template point cloud data after data preprocessing is a two-dimensional array .

[0040] Similarly, step S12 should specifically include: S121, filter the target point cloud data and remove r <3 or r =0 coordinate point.

[0041] S122, coordinate conversion is performed on the filtered target point cloud data, and the polar coordinates r , i and f Convert to rectangular coordinates x and y , the conversion relationships include: ; ; S123, decentralize the target point cloud data after coordinate conversion, 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 target point cloud data after data preprocessing is a two-dimensional array .

[0042] In the above step S04, KD-Tree is the abbreviation of K-Dimensional Tree, which is a data structure for segmenting k-dimensional data space and can be applied to the search of key data in multidimensional space. KD-Tree can effectively improve search efficiency and reduce the requirements of positioning methods on hardware performance.

[0043] The basic construction process of KD-Tree is as follows: Select the split dimension: Select the dimension with the largest variance from all dimensions as the split dimension (or preset an initial split dimension yourself); Select median: select the median of the coordinate points on this dimension as the split point; Split data: divide the coordinate points into two parts according to the split dimension, and recursively construct the left and right subtrees respectively; Recursive construction: Repeat the above process for the left and right subtrees until each leaf node contains only one coordinate point.

[0044] In some embodiments, step S2 includes: S21. Obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data.

[0045] S22. Calculate displacement estimation based on the shortest distance.

[0046] S23. Calculate an offset based on the displacement estimate.

[0047] 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.

[0048] If the displacement estimation is calculated only once, the error will be large. By introducing the concept of offset to iteratively calculate the displacement estimation, the displacement estimation result can be made more accurate. A more accurate displacement estimation can make the calculation result of the robot's current position more accurate, thereby obtaining higher positioning accuracy.

[0049] Furthermore, step S2 specifically includes: S20, estimate the displacement (Δx,Δy) Initialization, the calculation formula includes: ; .

[0050] 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 } .

[0051] Among them, the shortest distance d i The shortest distance used is Manhattan distance, which is d i = dx i + day i The calculation amount of Manhattan distance is relatively small, which can reduce the requirements of the positioning method on hardware performance.

[0052] 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, and the updated relationship includes: ; ; in, med() Represents the median of an array. (Δx,Δy) represents the current displacement estimate, (Δx',Δy') Represents the new displacement estimate.

[0053] This embodiment calculates the displacement estimate based on the median of the array, and uses the entire array as a reference, which can reduce the deviation caused by only referring to a small amount of data; and the use of the median can reduce the impact of extreme values ​​compared to the average, thereby improving the accuracy of the result. In addition, since our method uses the median as the displacement estimate, it does not require too much point cloud data to reflect the displacement of the robot, that is, it does not require a high-resolution radar.

[0054] It can be understood that the new displacement estimate is the current displacement estimate in the subsequent steps, that is, the displacement estimate in the steps executed after step S22 is the new displacement estimate, and iteration is performed continuously. For example, in the same round of iteration, the displacement estimate in step S23 should be the new displacement estimate in step S22; the current displacement estimate in step S22 in the next round of iteration should be the new displacement estimate in step S22 in the previous round of iteration.

[0055] S23, calculating the offset based on the displacement estimation, the calculation formula includes: ; in, D Indicates the offset.

[0056] 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.

[0057] The update relationship for updating the target point cloud data based on displacement estimation includes: ; ; in, Represents the current target point cloud data; Represents the new target point cloud data.

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

[0059] As mentioned above, if the displacement estimation is calculated only once, the error will be large. By introducing the concept of offset to iteratively calculate the displacement estimation, the calculation can be more accurate. Figure 3-5 As shown, Figure 3 It is a point cloud image that iteratively calculates the displacement estimate once. Figure 4 The point cloud image of displacement estimation is iteratively calculated three times. Figure 5 The point cloud image is calculated 24 times for displacement estimation. Figure 3-5In the figure, the point cloud represented by multiple dots "..." is the point cloud data scanned before the robot moves, and the point cloud represented by multiple circles "..." is the point cloud data scanned after the robot moves. The better the overlap of the two point clouds, the more accurate the displacement estimation. Figure 3-5 It can be seen that the displacement estimation accuracy of iteration 24, iteration 3 and iteration 1 decreases in turn. Figure 6 is the displacement estimate obtained at different iteration times, where the above figure is Δx The following figure shows the value of Δy As can be seen from the figure, with the increase of the number of iterations, the displacement estimation gradually approaches a fixed value, that is, the displacement estimation basically does not change after a finite number of iterations. At this time, it can be considered that the accuracy of the displacement estimation meets the requirements. This embodiment determines whether the displacement estimation is accurate enough by the offset. When the offset is less than the preset offset threshold, the displacement estimation is considered to be accurate enough, and the loop ends at this time.

[0060] In this embodiment, step S3 specifically includes: Displacement-based 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: ; .

[0061] In some embodiments, step S3 further includes: S4. 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.

[0062] Wherein, updating 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.

[0063] After calculating the current position of the robot once, if you need to continuously obtain the current position of the robot in real time, you need to collect point cloud data as the target point cloud data to continue the cycle calculation. Before collecting 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 far relative to its initial position. At this time, the initial position of the robot and the template point cloud data are updated before the next round of iteration, which can make the positioning accuracy more accurate.

[0064] The judgment standard for the threshold value in the present invention is whether it is "less than". It can be understood that "less than" can also be changed to "less than or equal to" according to needs. Such simple changes should be within the scope of protection of the present invention.

[0065] This embodiment also provides a positioning device based on point cloud data, please refer to Figure 7 , the positioning device comprises: Collection module 1, used for collecting a circle of point cloud data as target point cloud data; 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 estimation based on the shortest distance; The position calculation module 3 is used to calculate the current position of the robot based on the displacement estimation and the initial position of the robot.

[0066] In some embodiments, the acquisition module 1 includes: The template collection unit 11 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; The target acquisition unit 12 is used to acquire a circle of point cloud data as target point cloud data.

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

[0068] In some embodiments, the positioning device further comprises: Position initialization module 5 is used to initialize the robot's initial position (x 0 ,y 0 ) Initialize to (0,0) ; A data preprocessing module 6 is used to perform data preprocessing on the collected point cloud data; The template storage module 7 is used to store the template point cloud data after data preprocessing in the form of KD-Tree.

[0069] In some embodiments, the displacement estimation module 2 includes: A 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; An estimation calculation unit 22, configured to calculate a displacement estimation based on the shortest distance; An offset calculation unit 23, configured to calculate an offset based on the displacement estimate; 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 the search unit 21 is returned to continue execution.

[0070] In some embodiments, the data preprocessing module 6 includes: 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 , i and f 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 decentralized unit decentralizes the point cloud data after coordinate conversion. 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.

[0071] The data format of the point cloud data after data preprocessing is a two-dimensional array (x ci ,y ci ) ; Template point cloud data is superscript 0 Identification, target point cloud data is superscript 1 Logo.

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

[0073] In some embodiments, the retrieval unit 21 is specifically used to retrieve each coordinate point in the target point cloud data in the template point cloud data stored in the form of KD-Tree to obtain the shortest distance between the coordinate point 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 } .

[0074] In some embodiments, the estimation calculation unit 22 is specifically configured to calculate the shortest distance array in the x direction based on the x-direction shortest distance array. {dx} i } and the shortest distance array in the y direction {dy} i } , update the displacement estimate, and the updated relationship includes: ; ; in, med() Represents the median of an array. (Δx,Δy) represents the current displacement estimate, (Δx',Δy') Represents the new displacement estimate.

[0075] In some embodiments, the calculation formula of the offset calculation unit 23 includes: ; in, D Indicates the offset.

[0076] In some embodiments, the updating relationship of updating the target point cloud data based on the displacement estimation in the offset comparison unit 24 includes: ; ; in, Represents the current target point cloud data; Represents the new target point cloud data.

[0077] In some embodiments, the calculation formula of the position calculation module 3 includes: ; .

[0078] in, (x,y) Indicates the current position of the robot.

[0079] In some embodiments, the positioning device also includes an estimation comparison module 4, which is used to compare whether the displacement estimation 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.

[0080] Wherein, updating the template point cloud data includes: storing the target point cloud data after data preprocessing in the data preprocessing module 6 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 the position calculation module 3 as the new initial position; In some embodiments, the positioning device includes a laser radar, a posture 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 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.

[0081] All relevant contents of each step involved in the above positioning device embodiment can be referred to in the corresponding description in the relevant contents of the positioning method, and will not be further elaborated here.

[0082] This embodiment divides the positioning device into functional modules according to the above method examples. The functional modules can be implemented in the form of hardware, or the corresponding software can be implemented by hardware. Whether a 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 be beyond the scope of this application.

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

[0084] This embodiment further provides a computer storage medium, in which computer instructions are stored. When the computer instructions are executed on an electronic device, the electronic device executes the aforementioned positioning method.

[0085] 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 embodiment of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium, including several instructions to enable a device (which can be a single-chip microcomputer, chip, etc.) or a processor (processor) to perform all or part of the steps of the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read only memory (ROM), random access memory (RAM), disk or optical disk and other media that can store program code.

[0086] 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 repeated here.

[0087] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions 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; 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: The step S2 comprises: S21, obtaining the shortest distance between each coordinate point in the target point cloud data and the template point cloud data; S22, calculating displacement estimation based on the shortest distance; 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.

3. The positioning method based on point cloud data according to claim 2, 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.

4. The positioning method based on point cloud data according to claim 3, 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.

5. The positioning method based on point cloud data according to claim 4, characterized in that: Before step S21, the following steps are also included: S20, estimate the displacement (Δx,Δy) Initialization, the calculation formula includes: ； ； The step S21 comprises: Each coordinate point in the target point cloud data is retrieved in 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 } ; The step S22 comprises: 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, and the updated relationship 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 of step S23 includes: ； in, D Indicates the offset; The updating relationship of updating the target point cloud data based on the displacement estimation in step S24 includes: ； ； in, Represents the current target point cloud data; Represents the new target point cloud data.

6. The positioning method based on point cloud data according to claim 3, 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 update 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; The updating of the initial position of the robot includes: taking the current position of the robot in step S3 as a new initial position.

7. 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; The position calculation module is used to calculate the current position of the robot based on the displacement estimation and the initial position of the robot.

8. The positioning device based on point cloud data according to claim 7, 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) ; The displacement estimation module comprises: A retrieval unit, used to obtain the shortest distance between each coordinate point in the target point cloud data and the template point cloud data; An estimation calculation unit, used for calculating a displacement estimation based on a shortest distance; 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.

9. The positioning device based on point cloud data according to claim 8, 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; The displacement estimation module further includes an estimation initialization unit executed before the retrieval unit, wherein the estimation initialization unit is used to initialize the displacement estimation (Δx,Δy) Initialization, the calculation formula includes: ； ； The retrieval unit is specifically used to retrieve each coordinate point in the target point cloud data in the template point cloud data stored in the form of KD-Tree to obtain the shortest distance between the coordinate point 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 } ; The estimation calculation unit is specifically used to calculate 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, and the updated relationship 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 of the offset calculation unit includes: ； in, D Indicates the offset; The updating relationship of updating the target point cloud data based on the displacement estimation 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 updates 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; The updating of 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 6.

Images