Single target positioning method and device based on two-dimensional laser radar and multi-algorithm fusion

Abstract

The invention provides a single target positioning method and device based on two-dimensional laser radar and multi-algorithm fusion, and belongs to the technical field of target positioning. The method comprises the following steps: acquiring point cloud data by using a detection device comprising a two-dimensional laser radar, and converting the point cloud data into two-dimensional coordinate data to process an abnormal value; constructing an autonomous algorithm decision algorithm, and automatically selecting any two of a clustering algorithm, a distance difference algorithm, an equiangular area difference method and a boundary fitting distance difference method according to current features to calculate target positioning; verifying and correcting positioning output with inconsistent results through a third-class algorithm; whether the carrying platform is located in the center is judged through the point cloud distance, and extra center algorithm verification is carried out when the carrying platform is located in the center; the device comprises a two-dimensional laser radar module, a microprocessor module, a data storage module and a data display module. According to the method, the accurate positioning of the single target in the approximate regular closed space is realized, the requirement on the computing power of equipment is reduced, and the adaptability of embedded deployment is improved.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of target positioning, in particular to a single target positioning method and device based on two-dimensional laser radar and multi-algorithm fusion. BACKGROUND

[0002] The positioning technology based on laser radar is a key decision-making data source and technical means for robot inspection, cruising, tracking and autonomous decision-making, and the positioning of a target is realized through the calculation and processing of point cloud data returned by the laser radar. At present, target positioning mainly includes an external information positioning scheme based on GPS and Beidou, a visual positioning scheme and an internal information processing positioning scheme based on laser radar. The visual positioning scheme includes a positioning scheme based on a traditional RGB image, a target positioning scheme based on an infrared image, a target positioning scheme based on a depth image and a positioning scheme based on a multi-spectral image. The positioning scheme based on laser radar includes a target positioning scheme based on a three-dimensional laser radar and a target positioning scheme based on a two-dimensional laser radar.

[0003] However, the visual positioning and the three-dimensional laser radar positioning need to process a large amount of data and have high calculation complexity, which puts strict requirements on the algorithm power of embedded devices and mobile terminals, and it is difficult to deploy them on low-power devices. The traditional two-dimensional laser radar positioning algorithm has a positioning blind area, cannot effectively identify near-boundary targets, is easily disturbed by noise and outliers, and has insufficient positioning accuracy and stability. SUMMARY

[0004] The purpose of the application is to provide a single target positioning method and device based on two-dimensional laser radar and multi-algorithm fusion, which realizes the accurate positioning of a single target in a closed and approximately regular environment by fusing a geometric positioning algorithm and a traditional machine learning algorithm, solves the problem that traditional algorithms cannot position near-boundary targets, optimizes the calculation scheme, and improves the target positioning accuracy and deployment efficiency in embedded devices.

[0005] To achieve the above purpose, the application provides a single target positioning method based on two-dimensional laser radar and multi-algorithm fusion, which comprises the following steps: Step S1: using a detection device containing a two-dimensional laser radar to collect original laser radar data, calculating the angular resolution of the two-dimensional laser radar, and converting the original laser radar data into polar coordinate data; Step S2: converting the polar coordinate data into two-dimensional coordinate data in a Cartesian coordinate system, intercepting the two-dimensional coordinate data within a target angle range, and obtaining a data packet; Step S3: performing outlier processing on the two-dimensional coordinate data in the data packet, performing algorithm compensation on the outliers, obtaining data after outlier processing, performing smoothing and filtering operations on the data after outlier processing, obtaining processed point cloud data, and fitting a polygon contour through the Douglas-Peucker algorithm on the processed point cloud data; Step S4: constructing an autonomous algorithm decision algorithm through polar coordinate distance difference summation, presetting a threshold of the polar coordinate distance difference sum, presetting an algorithm parameter, comparing the polar coordinate distance difference sum and the threshold of the polar coordinate distance difference sum, determining whether the preset algorithm parameter is a simplified or normal mode, and automatically selecting any two from the clustering algorithm, the distance difference algorithm, the isometric area difference method, and the boundary fitting distance difference method according to the comparison result and the determination result; Step S5: calculating target positioning results by using the selected two algorithms respectively; Step S6: verifying and correcting target positioning results with inconsistent results through the third type of algorithm; Step S7: based on the fitted polygon, randomly sampling point cloud data equal in number to the length of the fitted polygon, calculating point cloud distances, determining the position of the detection device, verifying the target position through the diagonal distance algorithm, and outputting the positioning result.

[0006] Preferably, in step S2, the polar coordinate data is converted into two-dimensional coordinate data in the Cartesian coordinate system, and the conversion formula is: ; ; wherein, is the horizontal coordinate of the two-dimensional coordinate data, is the vertical coordinate of the two-dimensional coordinate data, is the distance measured by the two-dimensional laser radar, is the azimuth angle.

[0007] Preferably, in step S3, the following steps are included: Step S31: performing outlier processing on the two-dimensional coordinate data in the data packet, fitting the zero value based on the average value of the polar coordinate data of the previous and next two non-zero values and adding, obtaining data after outlier processing, and the calculation formula is: ; ; wherein, is the iteration difference value, is the initial value, is the final value, is the value of the current zero value after algorithm compensation, is the index corresponding to the zero value. Step S32: smoothing and filtering the data after the outlier processing to obtain the processed point cloud data; Step S33: connecting adjacent points in the processed point cloud data and fitting a polygon based on the Douglas-Peucker algorithm.

[0008] Preferably, in step S33, the following steps are included: Step S331: sorting the processed point cloud data, connecting the first point and the last point after sorting in a straight line to obtain a straight line L; Step S332: calculating the perpendicular distance of all intermediate points to the straight line L, finding the point with the maximum perpendicular distance, denoted as the maximum deviation point, recording the two-dimensional coordinate data of the maximum deviation point and the maximum perpendicular distance corresponding to the maximum deviation point; Step S333: presetting the perpendicular distance threshold of the maximum deviation point, judging the size of the maximum perpendicular distance and the perpendicular distance threshold, if the maximum perpendicular distance is greater than the perpendicular distance threshold, retaining the maximum deviation point, and dividing the straight line L into two line segments, the first point to the maximum deviation point and the maximum deviation point to the last point; Step S334: repeating step S333 for the two divided line segments respectively, and recursively until the maximum perpendicular distance is not greater than the perpendicular distance threshold, and the recursion ends; Step S335: connecting all retained points in order to form a fitted polygon, and outputting the shape information of the fitted polygon.

[0009] Preferably, in step S4, the following steps are included: Step S41: randomly extracting sample points from the processed point cloud data, obtaining polar coordinate data of the sample points, subtracting polar coordinate distance values in the sample point polar coordinate data in turn to obtain polar coordinate distance difference values, summing the polar coordinate distance difference values to obtain a polar coordinate distance difference sum; Step S42: presetting a threshold for the polar coordinate distance difference sum, presetting an algorithm parameter, comparing the size of the polar coordinate distance difference sum and the threshold for the polar coordinate distance difference sum, and determining whether the preset algorithm parameter is a simplification or a normal mode; Step S43: automatically selecting any two from the clustering algorithm, the distance difference algorithm, the equal angle area difference method and the boundary fitting distance difference method according to the comparison result and the determination result.

[0010] Preferably, in step S5, the clustering algorithm locates the target position by calculating the Euclidean distance between the processed point cloud data through the double-pointer algorithm, clustering according to the threshold, and determining the target position after removing the peripheral clusters and noise clusters; the distance difference algorithm locates the target position by calculating the distance difference between adjacent points in the processed point cloud data, and determining the target boundary according to the maximum value and the second maximum value corresponding to the angle; the equal angle area difference method locates the target position by calculating the area of the fan-shaped area at a fixed angle interval, and determining the target position according to the maximum value of the adjacent area difference; and the boundary fitting distance difference method locates the target position by calculating the minimum distance from the point cloud to each boundary based on the straight line equation of the fitted polygon boundary, and determining the target position according to the maximum value corresponding to the point.

[0011] Preferably, in step S6, the third type of algorithm does not select any one of the two algorithms, and the target positioning result is calculated and output by using the third type of algorithm.

[0012] Preferably, in step S7, the following steps are included: Step S71: randomly sampling the processed point cloud data equal in number to the length of the fitted polygon, calculating the point cloud distance, and judging the position of the detection device; Step S72: based on the relative angle of the laser radar, calculating the interval angle according to the shape information of the fitted polygon, and the calculation formula is: ; Wherein, is the number of polygon side lengths; Step S73: dividing the interval angle into intervals equal in number to the length of the side, and sequentially finding the point with the maximum point cloud distance in the interval as the vertex, and calculating the distance between the relative vertices; Step S74: outputting the final positioning result according to the distance between the relative vertices.

[0013] The application also provides a single target positioning device based on a two-dimensional laser radar and a multi-algorithm fusion, which comprises a two-dimensional laser radar module, a microprocessor module, a data storage module and a data display module. The two-dimensional laser radar module comprises a two-dimensional laser radar unit, a data transmission interface unit and an angle-distance measurement unit. The microprocessor module comprises a central processing unit, an algorithm operation unit, a data conversion unit and a logic control unit, and is in communication connection with the two-dimensional laser radar module. The data storage module comprises a temporary storage unit, a persistent storage unit and a data cache unit, and is in communication connection with the two-dimensional laser radar module. The data display module comprises a display screen and a graphics rendering unit, and is in communication connection with the microprocessor module.

[0014] Preferably, the device is located in a closed regular environment, and the height of the two-dimensional laser radar is lower than the target to be measured.

[0015] Therefore, the application proposes a single target positioning method and device based on two-dimensional laser radar and multi-algorithm fusion, which has the following beneficial effects: (1) The application can be applied to embedded devices, robots, and inspection equipment to achieve accurate and stable single target positioning. Compared with traditional star-based positioning, it can avoid signal attenuation problems in indoor environments. Compared with image-based or three-dimensional laser radar data positioning, it can reduce the demand for algorithm power and power consumption, and is suitable for algorithm-limited device deployment and transplantation.

[0016] (2) The application introduces multiple positioning algorithms, including geometric property-based algorithms, radar reflection principle-based algorithms, and machine learning-based algorithms. The multiple algorithms are enabled and disabled by an algorithm decision unit. After independent calculation, each algorithm votes. If the opinions are consistent, the output is output. If the opinions are inconsistent, a third-party algorithm is introduced for auxiliary calculation.

[0017] (3) The application introduces data filtering algorithms and outlier fitting algorithms, which greatly reduce the errors and abnormalities caused by noise, errors, and outliers. Compared with traditional algorithms, it can achieve near-boundary target positioning (such as near walls and fences), and can achieve efficient and fast result output, meeting the needs of high real-time tasks. BRIEF DESCRIPTION OF DRAWINGS

[0018] Figure 1 is a flowchart of a single target positioning method based on two-dimensional laser radar and multi-algorithm fusion; Figure 2 is a positioning result schematic diagram of positioning the target through a clustering algorithm; Figure 3 is a positioning result schematic diagram of positioning the target through a distance difference algorithm; Figure 4 is a positioning result schematic diagram of positioning the target through an isometric area difference method; Figure 5 is a positioning result schematic diagram of positioning the target through a boundary fitting distance difference method; Figure 6 is a structural diagram of a single target positioning device based on two-dimensional laser radar and multi-algorithm fusion; Figure 7 is a schematic diagram of a point cloud pattern in a closed regular environment obtained using a two-dimensional laser radar. DETAILED DESCRIPTION

[0019] The technical solutions of the application are further described below through the drawings and examples.

[0020] Unless otherwise defined, the technical or scientific terms used in this invention shall have the ordinary meaning as understood by one of ordinary skill in the art to which this invention pertains.

[0021] Example 1 like Figure 1 As shown, this invention provides a single-target localization method based on two-dimensional lidar and multi-algorithm fusion. It integrates three algorithms: geometric property algorithm, radar reflection principle algorithm, and machine learning algorithm. Multiple algorithms are used to analyze two-dimensional lidar data, and point cloud computing is used to obtain accurate target location, achieving single-target localization in a confined, rule-based environment. The method includes the following steps: Step S1: Use a detection device with a 2D lidar to collect raw lidar data, and use serial port protocol, IIC protocol or other protocols defined by the lidar manufacturer to read point cloud data, calculate the angular resolution of the 2D lidar, achieve the matching of angle and distance, and convert the raw lidar data into polar coordinate data. Step S2: Convert the polar coordinate data to two-dimensional coordinate data in the Cartesian coordinate system, extract the two-dimensional coordinate data within the target angle range, and obtain the data packet. The specific steps are as follows: Step S21: Convert the polar coordinate data to two-dimensional coordinate data in the Cartesian coordinate system. The conversion formula is as follows: ; ; in, The x-coordinate of the two-dimensional coordinate data. The vertical coordinate of the two-dimensional coordinate data. The distance measured by a two-dimensional lidar. It is the azimuth angle; Step S22: Truncate the converted two-dimensional coordinate data. If there are no special range requirements, truncate the values ​​with azimuth angles exceeding 360°, and package the two-dimensional coordinate data with azimuth angles within 360° to obtain a data package and output it. If it is necessary to truncate part of the data due to obstruction or operation requirements, set the target angle range of the azimuth angle, delete the data outside the target angle range, package the remaining two-dimensional coordinate data to obtain a data package and output it. Step S3: Perform outlier processing on the 2D coordinate data in the data packet, compensate for outliers using an algorithm, obtain outlier-processed data, smooth and filter the outlier-processed data to obtain processed point cloud data, and fit the processed point cloud data to a polygon contour using the Douglas-Peucker algorithm, including the following steps: Step S31: Abnormal value processing is performed on the two-dimensional coordinate data in the data packet, the zero value is fitted by averaging the polar coordinate data based on the two non-zero values before and after the zero value, and the average value is added to obtain the data after abnormal value processing, and the calculation formula is: ; ; wherein, is the iteration difference value, is the initial value, that is, the first non-zero data before the zero value, is the final value, that is, the first non-zero data after the zero value, is the value of the current zero value after compensation by the algorithm, is the index corresponding to the zero value, and the index starts from 1; Step S32: Smoothing and filtering operations are performed on the data after abnormal value processing, wherein the filtering operations include distance filtering, statistical filtering, radius filtering, Gaussian filtering, bilateral filtering, voxel grid-based filtering, Kalman filtering and other commonly used point cloud filtering methods; distance filtering is adopted, by setting a maximum distance threshold and a minimum distance threshold, points greater than the maximum distance threshold or less than the minimum distance threshold will be removed, to obtain the processed point cloud data. Step S33: Adjacent points in the processed point cloud data are connected, and a polygon is fitted based on the Douglas-Peucker algorithm, including the following steps: Step S331: The processed point cloud data is sorted, and the first point A and the last point B after sorting are connected to obtain a straight line L1; Step S332: The perpendicular distance of all intermediate points to the straight line L1 is calculated, the point with the maximum perpendicular distance is found, which is recorded as the maximum deviation point C, and the two-dimensional coordinate data of the maximum deviation point C and the maximum perpendicular distance corresponding to the maximum deviation point C are recorded; Step S333: A perpendicular distance threshold of the maximum deviation point is preset, and the size of the maximum perpendicular distance and the perpendicular distance threshold is judged, if the maximum perpendicular distance is greater than the perpendicular distance threshold, the maximum deviation point C is retained, and the straight line L1 is divided into two line segments AC from the first point to the maximum deviation point and BC from the maximum deviation point to the last point; Step S334: The two line segments AC and BC after division are repeatedly processed by step S333, and recursion is performed until the maximum perpendicular distance is not greater than the perpendicular distance threshold, and the recursion is ended; Step S335: All retained points are connected in order to form a fitted polygon, and the shape information of the fitted polygon is output.

[0022] Step S4: constructing an autonomous algorithm decision algorithm by polar coordinate distance difference summation, presetting a threshold of the polar coordinate distance difference summation, presetting an algorithm parameter, comparing the polar coordinate distance difference summation with the threshold of the polar coordinate distance difference summation, judging whether the preset algorithm parameter is a simplified mode or a normal mode, and automatically selecting any two from the clustering algorithm, the distance difference algorithm, the equiangular area difference method and the boundary fitting distance difference method according to the comparison result and the judgment result, the specific steps being as follows: Step S41: randomly extracting 10 sample points from the processed point cloud data, obtaining polar coordinate data of the sample points, subtracting polar coordinate distance values of the extracted 10 sample points in turn to obtain polar coordinate distance differences, summing the polar coordinate distance differences to obtain a polar coordinate distance difference summation; Step S42: setting a half of a diagonal line length of the fitted polygon and ten times of a difference between a distance from the geometric center of the fitted polygon to one of the side lengths as a threshold of the polar coordinate distance difference summation, presetting the algorithm parameter as mode, and comparing the polar coordinate distance difference summation with the threshold of the polar coordinate distance difference summation; Step S43: automatically selecting any two from the clustering algorithm, the distance difference algorithm, the equiangular area difference method and the boundary fitting distance difference method according to the comparison result and the judgment result, specifically: if the polar coordinate distance difference summation is less than the threshold of the polar coordinate distance difference summation, judging that the preset algorithm parameter is the simplified mode or the normal mode, when the polar coordinate distance difference summation is less than the threshold of the polar coordinate distance difference summation, the preset algorithm parameter is the simplified mode, and the clustering algorithm and the distance difference algorithm are selected, and when the polar coordinate distance difference summation is greater than or equal to the threshold of the polar coordinate distance difference summation, the preset algorithm parameter is the normal mode, and the clustering algorithm and the equiangular area difference method are selected; Step S43: automatically selecting any two from the clustering algorithm, the distance difference algorithm, the equiangular area difference method and the boundary fitting distance difference method according to the comparison result and the judgment result, specifically: if the polar coordinate distance difference summation is less than the threshold of the polar coordinate distance difference summation, judging that the preset algorithm parameter is the simplified mode or the normal mode, when the polar coordinate distance difference summation is less than the threshold of the polar coordinate distance difference summation, the preset algorithm parameter is the simplified mode, and the clustering algorithm and the distance difference algorithm are selected, and when the polar coordinate distance difference summation is greater than or equal to the threshold of the polar coordinate distance difference summation, the preset algorithm parameter is the normal mode, and the clustering algorithm and the equiangular area difference method are selected; Step S5: calculating target positioning results by using the selected two algorithms respectively; As shown in Figure 2 , the clustering algorithm positioning is to calculate the Euclidean distance between the processed point cloud data by a double-pointer algorithm, to cluster into clusters according to a threshold, to determine a target position after eliminating peripheral clusters and noise clusters, including the following steps: Step T1: obtaining the processed point cloud data, setting two lists and initializing them, storing the processed point cloud data and a retrieval identification symbol respectively, setting all points in the processed point cloud data to 0, that is, setting them as unretrieved; ​Step T2: the first untraversed point in the list of processed point cloud data is extracted as a seed point, a new cluster list is created and the seed point is stored as a point cloud set of classification 1, and the traversal index is set to 0, indicating that the first point in the class is currently being clustered and matched. The search identifier of the point is set to 1. Then, the Euclidean geometric distance between the other untraversed points in the list of processed point cloud data and the seed point in the cluster list of classification 1 is calculated. In order to reduce the performance overhead, the value calculated here is the square of the Euclidean geometric distance, and the calculation formula is: ; wherein, is the Euclidean geometric distance, is the Cartesian horizontal coordinate of the untraversed point, is the Cartesian vertical coordinate of the untraversed point, is the Cartesian horizontal coordinate of the seed point, is the Cartesian vertical coordinate of the seed point; Step T3: set the classification threshold If , the point is added to the corresponding list, and the point is deleted from the point cloud set. If , the search identifier remains unchanged, and the next unclassified point is traversed. Step T4: when all unclassified points have completed the calculation and matching, the list traversal index of the class is increased by 1, and steps T1-T3 are repeated. When the list traversal index of the class is equal to the number of points in the class minus 1, the clustering of the class is completed. Then, a point is extracted from the unclustered points in order as a new seed point to start a new class clustering, and steps T1-T4 are repeated.

[0023] When the clustering is completed, if there is a prior condition for the detected target, such as length, the class with the most points (regarded as a boundary or wall) can be deleted, and the class with too few points (regarded as noise) can be removed. The remaining target is the detection target. Then, based on the clustered point cloud, the target length and width can be calculated according to the extreme difference of x and y, and the relative angle range of the target can be calculated according to the average value of the point cloud angle, thereby realizing positioning.

[0024] In addition, the wall can also be screened according to the area. Since the point cloud area of the wall is huge, a larger point cloud area threshold can be set to screen and remove the wall and boundary.

[0025] As shown in Figure 3 , the distance difference algorithm positioning is to calculate the distance difference value of adjacent points in the processed point cloud data, and determine the target boundary according to the maximum value and the second maximum value corresponding to the angle. The specific steps are as follows: Step W1: Obtain the processed point cloud data, and calculate the distance difference between adjacent points in turn; Step W2: If there is data truncation, calculate the difference between adjacent points for the segmented continuous data in turn, and remove the data with less than 5 points to avoid interference; Step W3: Calculate the maximum and second maximum of the adjacent point difference; Step W4: If the maximum and second maximum are less than 5 cm, discard the result and return an empty result; Step W5: If the corresponding laser radar angle difference between the maximum and second maximum is less than 40°, it is considered to be the end of the same object mutation information, and the average of the corresponding angles of the maximum and second maximum is the positioning angle; If the condition is not met, the corresponding angle of the maximum is taken as the target positioning result.

[0026] Although this algorithm can quickly calculate the target position, it cannot effectively position if the target is located at the boundary or the periphery of the wall. Therefore, when the returned result is invalid or fails to verify, a third-party algorithm verification is needed.

[0027] As shown in Figure 4 , the equal angle area difference positioning method calculates the area of the fan-shaped group by fixed angle interval, and determines the target position by the maximum adjacent area difference. The specific steps are as follows: Step F1: Convert the point cloud data collected by the laser radar into polar coordinate data, i.e. (angle, distance) form; Step F2: Select an appropriate combination coefficient , and calculate the area of adjacent point combination with 1 group of points, for example, when 3 is selected, the 0th, 1st and 2nd points are bound as a group to calculate the area; Step F3: Calculate the fan-shaped area corresponding to each group of point clouds using the similar fan-shaped area calculation formula, the calculation formula is: ; Wherein, is the corresponding distance of the th point, is the corresponding distance of the th point, is the set combination coefficient, and the angle difference from the th point to the th point is calculated according to the angle resolution of the laser radar; Step F4: Calculate the difference of adjacent fan-shaped areas, and keep the maximum and second maximum of the absolute value of the difference; Step F5: If the maximum and second maximum of the absolute value of the adjacent area difference are less than 5, return an empty result; Step F6: If the difference between the maximum value and the second maximum value corresponding to the laser radar angle is less than 40°, it is considered that the two end mutation information of the same object, and the average of the angles corresponding to the maximum value and the second maximum value is taken as the positioning angle; if the condition is not met, the angle corresponding to the maximum value is taken as the target positioning result; As shown in Figure 5 , the boundary fitting distance difference positioning is based on the fitting polygon boundary straight line equation, calculates the minimum distance from the point cloud to each boundary, takes the maximum value corresponding point to determine the target position, and the specific steps are as follows: Step H1: obtaining the processed point cloud data; Step H2: using Douglas-Peucker algorithm to perform approximate polygon fitting on the point cloud data, selecting the fitting parameter as 0.05 times of the side length as the threshold, and fitting the point cloud into a polygon; Step H3: according to the shape of the fitted polygon, selecting two fitting vertices, and calculating the straight line equation of the boundary based on the coordinates of the two fitting vertices, assuming that the equation of the straight line is , the Cartesian coordinates of the two fitting vertices are substituted into, and the parameter is calculated. Step H4: according to the shape characteristics of the regular polygon, the straight line equations of the remaining boundaries are calculated, the angle between the side and the geometric center of the regular polygon is the angle between the adjacent sides, according to the angle and the triangular formula, the slope of the adjacent side is inferred based on the first known straight line equation, that is, the parameter is calculated, and the parameter is determined based on the common vertex of the two sides. Step H5: repeating step H4 until the mathematical equations of all fitting polygon sides are inferred; Step H6: according to the point-to-line distance formula, the distance values of each point in the point cloud to each boundary are calculated, the minimum value of the distance values of each point to each boundary is taken as the boundary distance, and the boundary distances of each point are calculated in turn and stored in a list, and the distance formula is: ; wherein, is the distance from the point to the line, , , is the straight line equation coefficient, is the horizontal coordinate of the point cloud data, is the vertical coordinate of the point cloud data; Step H7: taking the maximum value of all boundary distances, the point direction corresponding to the maximum value is the target direction.

[0028] Step S6: verifying and correcting the target positioning result with inconsistent results by a third type of algorithm, wherein the third type of algorithm refers to any one of the two selected algorithms, and the target positioning result is calculated by using the third type of algorithm, and if the target positioning result is similar to the target positioning result of any one of the two selected algorithms, the target positioning result is output.

[0029] Step S7: based on the fitted polygon, randomly sampling the processed point cloud data equal to the number of side lengths of the fitted polygon, calculating the point cloud distance, judging whether the position of the detection device is located near the geometric center of the detection scene, verifying the target position by the diagonal distance algorithm, and outputting the positioning result, and the specific steps are as follows: Step S71: randomly selecting 5 processed point cloud data, selecting 4 processed point cloud data at intervals of 90 degrees, calculating the point cloud distance, and if the distances of three of them are approximately equal, it is determined that the device is located near the geometric center of the detection scene; Step S72: based on the relative angle of the laser radar, the interval angle is calculated according to the shape information of the fitted polygon, and the calculation formula is: ; Wherein, is the number of side lengths of the polygon; Step S73: sequentially selecting , , …, find the point cloud distance of the largest point as the vertex, and calculate the distance between the relative vertices with an interval angle of approximately 180°; Step S74: according to the distance between the relative vertices, the final positioning result is output, specifically: if the distance is consistent, it is judged whether the positioning angle of the current detection target is on the diagonal line, if not, it is considered that the result is correct, and the output is output; if the distance is inconsistent, the relative angle of the point cloud with the shorter distance is taken as the positioning result output.

[0030] Embodiment Two As shown in Figure 6 , the application also provides a single target positioning device based on a two-dimensional laser radar and a multi-algorithm fusion, comprising a two-dimensional laser radar module, a microprocessor module, a data storage module and a data display module. The two-dimensional laser radar module is a core perception component of the device, responsible for collecting point cloud data in a closed and approximately regular environment, and providing raw measurement information for target positioning. It includes a two-dimensional laser radar unit, a data transmission interface unit, and an angle-distance measurement unit. The two-dimensional laser radar unit collects point cloud data by emitting a laser beam and receiving reflected signals, and obtains distance and angle information of objects in the environment. It converts the point cloud data into polar coordinate data. The data transmission interface unit transmits the collected point cloud data and the converted polar coordinate data to the microprocessor module according to a predetermined protocol. The angle-distance measurement unit scans 360° or a specified angle range with a fixed angular resolution, and outputs continuous distance-angle correspondence data.

[0031] The microprocessor module undertakes the core data processing and decision-making tasks of the device, and is the execution center of multi-algorithm fusion positioning. It dynamically optimizes algorithm decisions based on historical data from the storage module and real-time inputs, and realizes accurate positioning of a single target. It includes a central processing unit, an algorithm operation unit, a data conversion unit, and a logic control unit. The central processing unit is a CPU, FPGA, or MCU, which coordinates the work of various modules. After the raw data collected by the two-dimensional laser radar module is converted and processed by the microprocessor module, it is stored in the data storage module on one hand, and visualized output by the data display module on the other hand, and the computing resources are dispatched. The data conversion unit converts the polar coordinate data output by the laser radar into Cartesian coordinate system coordinates , providing a unified data format for subsequent algorithms. The algorithm operation unit performs data preprocessing (outlier processing, smoothing, and filtering), multi-algorithm positioning (clustering algorithm, distance difference method, equal angle area difference method, and boundary fitting distance difference method), result verification, and logic correction. The logic control unit triggers data storage, display update, and other operations based on algorithm decision results, and controls the device workflow.

[0032] The data storage module is responsible for temporary caching and long-term storage of data, ensuring the continuity of algorithm calculation and the traceability of results. It includes a temporary storage unit, a persistent storage unit, and a data cache unit. The temporary storage unit includes DDR2, DDR3, DDR4, or DDR5, and caches real-time collected point cloud data and intermediate results of algorithm calculation, such as fitting polygon parameters and clustering cluster information, and supports high-speed read and write. The persistent storage unit includes EEPROM or Flash, and saves preprocessed key data, positioning result logs, and algorithm parameters such as filtering threshold and clustering threshold λ, ensuring that data is not lost after power failure. The data cache unit can alleviate the speed difference between data collection and processing, and avoid data overflow or interruption.

[0033] The data display module realizes visualization of the positioning process and result, and comprises a display screen and a graphic rendering unit; the display screen unit displays a point cloud distribution diagram, a fitted environment contour, a target positioning angle and coordinate, and algorithm running state information in real time; and the graphic rendering unit converts the original data into an intuitive graphical interface, such as a point cloud scatter diagram and a target marking box, and improves readability.

[0034] The application will be further described below through specific implementation cases.

[0035] In order to verify the rationality and effectiveness of the method proposed in the application, systematic experimental research is carried out on a synthetic noise data set.

[0036] Step S1: original laser radar data is collected by using a detection device containing a two-dimensional laser radar, wherein a point cloud data pattern is as shown in Figure 7 The serial port protocol, IIC protocol or other protocol defined by the radar manufacturer is used for reading the point cloud data, the angular resolution of the two-dimensional laser radar is calculated, the matching of the angle and the distance is realized, the original laser radar data is converted into polar coordinate data, and the polar coordinate data is converted into two-dimensional coordinate data in a Cartesian coordinate system. Step S2: the two-dimensional coordinate data in the Cartesian coordinate system is obtained by intercepting the two-dimensional coordinate data in a target angle range, and the data packet is obtained, and the specific steps are as follows: Step S21: the two-dimensional coordinate data in the Cartesian coordinate system is converted from the polar coordinate data, and the conversion formula is as follows: ; ; Wherein, is the horizontal coordinate of the two-dimensional coordinate data, is the vertical coordinate of the two-dimensional coordinate data, is the distance measured by the two-dimensional laser radar, is the azimuth angle. Step S22: the converted two-dimensional coordinate data is truncated, if there is no special range requirement, the value of the azimuth angle exceeding 360° is truncated, the two-dimensional coordinate data with the azimuth angle within 360° is packaged, and the data packet is obtained and output; if part of the data needs to be truncated due to shielding or operation requirements, the target angle range of the azimuth angle is set, the data outside the target angle range is deleted, the remaining two-dimensional coordinate data is packaged, and the data packet is obtained and output. Step S3: the two-dimensional coordinate data in the data packet is subjected to abnormal value processing, the abnormal value is compensated by an algorithm, the data after abnormal value processing is subjected to smoothing and filtering operation, the processed point cloud data is obtained, the processed point cloud data is fitted into a polygon contour by a Douglas-Peucker algorithm, and the following steps are included: Step S31: Abnormal value processing is performed on the two-dimensional coordinate data in the data packet, the zero value is fitted by averaging the polar coordinate data based on the two non-zero values before and after the zero value and adding, and the abnormal value processed data is obtained, and the calculation formula is: ; ; wherein, is an iterative difference value, is a first non-zero data before the zero value, i.e., the initial value, is a last non-zero data after the zero value, i.e., the final value, is a value of the current zero value after compensation by the algorithm, is an index corresponding to the zero value, and the index starts from 1; Step S32: Smoothing and filtering operations are performed on the abnormal value processed data, distance filtering is adopted, by setting a maximum distance threshold and a minimum distance threshold, points greater than the maximum distance threshold or less than the minimum distance threshold will be removed, and the processed point cloud data is obtained; Step S33: Adjacent points in the processed point cloud data are connected, and a polygon is fitted based on the Douglas-Peucker algorithm, including the following steps: Step S331: The processed point cloud data is sorted, and the first point A and the last point B after sorting are connected by a straight line L1; Step S332: The perpendicular distance of all intermediate points to the straight line L1 is calculated, the point with the maximum perpendicular distance is found, which is recorded as the maximum deviation point C, and the two-dimensional coordinate data of the maximum deviation point C and the maximum perpendicular distance corresponding to the maximum deviation point C are recorded; Step S333: A perpendicular distance threshold of the maximum deviation point is preset, and the size of the maximum perpendicular distance and the perpendicular distance threshold is judged, if the maximum perpendicular distance is greater than the perpendicular distance threshold, the maximum deviation point C is retained, and the straight line L is divided into two line segments AC and BC from the first point to the maximum deviation point and from the maximum deviation point to the last point; Step S334: Steps S333 are repeated for the two divided line segments AC and BC respectively, and recursion is performed until the maximum perpendicular distance is not greater than the perpendicular distance threshold, and the recursion ends; Step S335: All retained points are connected in order to form a fitted polygon, and the shape information of the fitted polygon is output.

[0037] Step S4: An autonomous algorithm decision algorithm is constructed by polar coordinate distance difference summation, a threshold of the polar coordinate distance difference summation is preset, an algorithm parameter is preset, a comparison is made between the polar coordinate distance difference summation and the threshold of the polar coordinate distance difference summation, it is judged that the preset algorithm parameter is a simplified mode or a normal mode, and according to the comparison result and the judgment result, any two of the clustering algorithm, the distance difference algorithm, the equal angle area difference method and the boundary fitting distance difference method are automatically selected, and the specific steps are as follows: Step S41: Ten sample points are randomly extracted from the processed point cloud data, polar coordinate data of the sample points are obtained, polar coordinate distance values of the extracted ten sample points are sequentially subtracted to obtain polar coordinate distance difference values, the polar coordinate distance difference values are summed to obtain a polar coordinate distance difference summation; Step S42: The threshold of the polar coordinate distance difference summation is set as ten times of the difference between half of the diagonal line length of the fitted polygon and the distance from the geometric center of the fitted polygon to one of the side lengths, the preset algorithm parameter is mode, and a comparison is made between the polar coordinate distance difference summation and the threshold of the polar coordinate distance difference summation. Step S43: According to the comparison result and the judgment result, any two of the clustering algorithm, the distance difference algorithm, the equal angle area difference method and the boundary fitting distance difference method are automatically selected, and the specific steps are as follows: if the polar coordinate distance difference summation is less than the threshold of the polar coordinate distance difference summation, it is judged that the preset algorithm parameter is a simplified mode or a normal mode, when , the preset algorithm parameter is the simplified mode, the clustering algorithm and the distance difference algorithm are selected, when , the preset algorithm parameter is the normal mode, the clustering algorithm and the equal angle area difference method are selected; if the polar coordinate distance difference summation is greater than or equal to the threshold of the polar coordinate distance difference summation, the clustering algorithm and the adjacent distance difference method are used. Step S5: The target positioning result is calculated by using the selected two algorithms, wherein the clustering algorithm positioning is to calculate the Euclidean distance between the processed point cloud data by a double-pointer algorithm, to cluster into clusters according to a threshold, to remove peripheral clusters and noise clusters, and to determine the target position; the distance difference algorithm positioning is to calculate the distance difference value between adjacent points in the processed point cloud data, to determine the target boundary according to the maximum value and the second maximum value corresponding to the angle; the equal angle area difference method positioning is to calculate the sector area by fixed angle interval grouping, and to determine the target position by using the maximum value of the adjacent area difference; the boundary fitting distance difference method positioning is to calculate the minimum distance from the point cloud to each boundary based on the boundary straight line equation of the fitted polygon, and to determine the target position corresponding to the maximum value.

[0038] Step S6: The target positioning result with inconsistent result is verified and corrected by the third type of algorithm, wherein the third type of algorithm refers to any one of the two algorithms not selected, the target positioning result is calculated by using the third type of algorithm, and if the target positioning result is similar to that of any one of the two selected algorithms, the target positioning result is output.

[0039] Step S7: based on the fitted polygon, randomly sampling the processed point cloud data equal to the number of polygon side lengths, calculating the point cloud distance, judging the position of the detection device, verifying the target position through the diagonal distance algorithm, and outputting the positioning result, the specific steps are as follows: Step S71: randomly selecting 5 processed point cloud data, selecting 4 processed point cloud data at intervals of 90 degrees, calculating the point cloud distance, and if the distances of three of them are approximately equal, it is determined that the device is located near the geometric center of the detection scene; Step S72: based on the relative angle of the laser radar, the interval angle is calculated according to the shape information of the fitted polygon, and the calculation formula is: ; Among them, is the number of polygon side lengths; Step S73: sequentially in , , …, find the point cloud distance of the largest point as the vertex, and calculate the distance between the relative vertices; Step S74: according to the distance between the relative vertices, output the final positioning result, specifically: if the distances are consistent, verify whether the current detection target positioning angle is on the diagonal line, if not, it is considered that the result is correct, and output; if the distances are inconsistent, the relative angle of the point cloud with the shorter distance is taken as the positioning result output.

[0040] It should be noted that the contents not elaborated in the present application are all prior art and are well known to those skilled in the art.

[0041] Therefore, the single target positioning method and device based on two-dimensional laser radar and multi-algorithm fusion proposed by the present application acquires point cloud data through a two-dimensional laser radar, converts to a Cartesian coordinate system, performs outlier compensation and data filtering; the Douglas-Peucker algorithm is used to construct an environmental polygon contour; two positioning algorithms are automatically selected based on the distance difference sum; inconsistent results are verified through the third algorithm, and diagonal line checking is performed for the center position. The present application realizes the positioning of targets within the boundary and near the boundary in a closed and approximately regular environment under limited computing power, solves the problem that traditional algorithms cannot position near-boundary targets, optimizes the calculation scheme, and improves the deployment efficiency in embedded devices.

[0042] It should be pointed out finally that the above examples are only used to illustrate the technical solutions of the present application but not to limit it, and although the present application has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solutions of the present application can still be modified or replaced equivalently, and these modifications or equivalent replacements should not make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present application.

Claims

1. A single target positioning method based on two-dimensional laser radar and multi-algorithm fusion, characterized in that, The method comprises the following steps: Step S1: collecting original laser radar data using a detection device comprising a two-dimensional laser radar, calculating the angular resolution of the two-dimensional laser radar, and converting the original laser radar data into polar coordinate data; Step S2: converting the polar coordinate data into two-dimensional coordinate data in a Cartesian coordinate system, intercepting two-dimensional coordinate data within a target angle range to obtain a data packet; Step S3: performing outlier processing on the two-dimensional coordinate data in the data packet, performing algorithm compensation on the outliers, obtaining processed data after outlier processing, performing smoothing and filtering operations on the processed data after outlier processing, obtaining processed point cloud data, and fitting a polygon contour from the processed point cloud data through a Douglas-Peucker algorithm; Step S4: constructing an autonomous algorithm decision algorithm through polar coordinate distance difference summation, presetting a threshold for the polar coordinate distance difference sum, presetting an algorithm parameter, comparing the polar coordinate distance difference sum and the threshold for the polar coordinate distance difference sum, determining whether the preset algorithm parameter is in a simplified or normal mode, and automatically selecting any two from a clustering algorithm, a distance difference algorithm, an equal angle area difference method, and a boundary fitting distance difference method according to the comparison result and the determination result; Step S5: calculating target positioning results using the selected two algorithms; Step S6: verifying and correcting target positioning results that are inconsistent through a third algorithm; Step S7: based on the fitted polygon, randomly sampling point cloud data equal in number to the length of the fitted polygon, calculating point cloud distances, determining the position of the detection device, verifying the target position through an angle distance algorithm, and outputting the positioning result.

2. The single target positioning method based on two-dimensional laser radar and multi-algorithm fusion according to claim 1, characterized in that: In step S2, the polar coordinate data is converted into two-dimensional coordinate data in a Cartesian coordinate system, and the conversion formula is: ； ； wherein is the abscissa of the two-dimensional coordinate data, is the ordinate of the two-dimensional coordinate data, is the distance of the two-dimensional laser radar measurement, is the azimuth angle.

3. The single target positioning method based on two-dimensional laser radar and multi-algorithm fusion according to claim 1, characterized in that: In step S3, the following steps are included: Step S31: performing outlier processing on the two-dimensional coordinate data in the data packet, fitting the outliers based on the average value of the polar coordinate data of the two non-zero values before and after the zero value, and adding the average value to obtain processed data after outlier processing, and the calculation formula is: ； ； wherein, is the iteration difference, is the initial value, is the final value, is the current zero value after compensation by the algorithm, is the zero value corresponding index; Step S32: performing smoothing and filtering operations on the processed data after outlier processing to obtain processed point cloud data; Step S33: connecting adjacent points in the processed point cloud data and fitting a polygon contour based on the Douglas-Peucker algorithm to obtain a fitted polygon.

4. The single target positioning method based on two-dimensional laser radar and multi-algorithm fusion according to claim 3, characterized in that: In step S33, the following steps are included: Step S331: sorting the processed point cloud data, connecting the first point and the last point after sorting in a straight line to obtain a straight line L; Step S332: calculating the perpendicular distance of all intermediate points from the straight line L, finding the point with the maximum perpendicular distance, denoted as a maximum deviation point, recording the two-dimensional coordinate data of the maximum deviation point and the maximum perpendicular distance corresponding to the maximum deviation point; Step S333: presetting a threshold for the perpendicular distance of the maximum deviation point, determining the size of the maximum perpendicular distance and the threshold for the perpendicular distance, and if the maximum perpendicular distance is greater than the threshold for the perpendicular distance, retaining the maximum deviation point and dividing the straight line L into two line segments from the first point to the maximum deviation point and from the maximum deviation point to the last point; Step S334: repeat step S333 for the two segmented line segments respectively, and perform recursion until the maximum vertical distance is not greater than the vertical distance threshold value, and the recursion ends; Step S335: connect all the reserved points in order to form a fitted polygon, and output the fitted polygon shape information.

5. The single target positioning method based on two-dimensional laser radar and multi-algorithm fusion according to claim 1, characterized in that: In step S4, the following steps are included: Step S41: randomly extract sample points from the processed point cloud data, obtain polar coordinate data of the sample points, sequentially subtract the distance values in the sample point polar coordinate data to obtain polar coordinate distance difference values, sum the polar coordinate distance difference values to obtain a polar coordinate distance difference sum; Step S42: preset a threshold value of the polar coordinate distance difference sum, preset an algorithm parameter, compare the polar coordinate distance difference sum with the threshold value of the polar coordinate distance difference sum, and determine whether the preset algorithm parameter is a simplification or a normal mode; Step S43: automatically select any two from the clustering algorithm, the distance difference algorithm, the equal angle area difference method, and the boundary fitting distance difference method according to the comparison result and the determination result.

6. The single target positioning method based on two-dimensional laser radar and multi-algorithm fusion according to claim 1, characterized in that: In step S5, the clustering algorithm positioning is to calculate the Euclidean distance between the processed point cloud data by a double-pointer algorithm, cluster into clusters according to a threshold value, and determine the target position after removing peripheral clusters and noise clusters; the distance difference algorithm positioning is to calculate the distance difference values of adjacent points in the processed point cloud data, and determine the target boundary according to the maximum value and the second maximum value corresponding to the angle; the equal angle area difference method positioning is to calculate the sector area by grouping at a fixed angle interval, and determine the target position by using the maximum value of the adjacent area difference; and the boundary fitting distance difference method positioning is to calculate the minimum distance from the point cloud to each boundary based on the boundary straight line equation of the fitted polygon, and determine the target position corresponding to the maximum value.

7. The single target positioning method based on two-dimensional laser radar and multi-algorithm fusion according to claim 1, characterized in that: In step S6, the third type of algorithm refers to any one of the two algorithms that are not selected, and the target positioning result is calculated and output by using the third type of algorithm.

8. The single target positioning method based on two-dimensional laser radar and multi-algorithm fusion according to claim 1, characterized in that: In step S7, the following steps are included: Step S71: randomly sample the processed point cloud data equal to the number of polygon side lengths, calculate the point cloud distance, and determine the position of the detection device; Step S72: based on the relative angle of the laser radar, calculate the interval angle according to the fitted polygon shape information, and the calculation formula is: ； wherein, is the number of polygon sides for the fit; Step S73: divide the interval angle into intervals equal to the number of polygon side lengths, and sequentially find the point with the maximum point cloud distance in the interval as the vertex, and calculate the distance between the relative vertices; Step S74: output the final positioning result according to the distance between the relative vertices.

9. A single target positioning device based on two-dimensional laser radar and multi-algorithm fusion, characterized in that: The device includes a two-dimensional laser radar module, a microprocessor module, a data storage module, and a data display module; The two-dimensional laser radar module includes a two-dimensional laser radar unit, a data transmission interface unit, and an angle-distance measurement unit; The microprocessor module includes a central processing unit, an algorithm operation unit, a data conversion unit, and a logic control unit, and is in communication connection with the two-dimensional laser radar module; The data storage module includes a temporary storage unit, a persistent storage unit, and a data cache unit, and is in communication connection with the two-dimensional laser radar module; The data display module includes a display screen and a graphics rendering unit, and is in communication connection with the microprocessor module.

10. The single target positioning device based on two-dimensional laser radar and multi-algorithm fusion according to claim 9, characterized in that: The device is located in a closed and regular environment, and the height of the two-dimensional laser radar is lower than the target to be measured.

Images