Identification positioning algorithm based on single-line laser radar and reflector

Abstract

The invention discloses a recognition and positioning algorithm based on a single-line laser radar and a reflector. The recognition and positioning algorithm comprises the following steps: S1, firstly defining coordinate systems of a mobile robot and the laser radar and the size and the installation position of the reflector; s2, measuring and determining intensity information returned by the laser radar in the reflector at a specific distance, and setting an intensity information threshold value and a distance threshold value at the same time; and S3, receiving the original point cloud of the laser radar, extracting all the reflectors by adopting a linear interpolation method, calculating the poses of the reflectors in the laser radar coordinate system, and finally converting the poses to the base coordinate system to obtain the accurate poses. According to the invention, the positioning precision of the single-line laser radar in a complex environment is improved by combining the information of the reflector; the arrangement of the reflectors can be adjusted according to the environmental complexity, so that the algorithm adaptability is enhanced; the single-line laser radar is small in data size, can quickly give out a positioning result, and is good in algorithm real-time performance. The high-reflection information of the reflector has a strong anti-interference capability, and the positioning result is more stable.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and identification, and in particular to an identification and positioning algorithm based on a single-line laser radar and a reflector. Background Art

[0002] Single-line LiDAR and reflectors are commonly used in scenarios such as positioning and identification of self-driving cars and robots. LiDAR uses the time-of-flight method to detect objects in the surrounding environment by sending a laser beam and receiving the laser reflected back. A reflector is a special surface that can reflect laser beams and is usually used to mark locations or as a positioning reference point. In terms of identification and positioning, single-line LiDAR can detect and track objects by analyzing the reflection data of the laser beam. Reflectors can reflect laser beams, causing them to produce obvious reflection signals in the sensor, thereby helping the system to more easily identify these boards and locate the locations they represent.

[0003] The existing recognition and positioning of mobile robots mainly include vision-based positioning and laser radar-based positioning. Vision-based positioning is greatly affected by environmental factors, resulting in low accuracy, while the price of multi-line laser radar is generally high. In order to improve the accuracy and stability of recognition and positioning, the present invention proposes a recognition and positioning algorithm based on single-line laser radar and reflector, which can obtain stable and accurate positioning posture. Summary of the invention

[0004] 1. Technical issues to be resolved

[0005] In view of the shortcomings of the existing technology, the present invention provides an identification and positioning algorithm based on a single-line laser radar and a reflector, which solves the problem that the identification and positioning of existing mobile robots mainly include two types: positioning based on vision and positioning based on laser radar. Positioning based on vision is greatly affected by environmental factors, resulting in low accuracy, while the price of multi-line laser radar is generally high.

[0006] (II) Technical solution

[0007] To achieve the above objectives, the present invention is implemented through the following technical solutions:

[0008] The technical solution of the present invention is: an identification and positioning algorithm based on a single-line laser radar and a reflector, comprising the following steps:

[0009] S1. First, define the coordinate system of the mobile robot and the laser radar as well as the size and installation position of the reflector;

[0010] S2, measuring and determining the intensity information returned by the laser radar in the reflector at a specific distance, and setting the intensity information threshold and the distance threshold;

[0011] S3, receiving the original point cloud of the LiDAR, using linear interpolation to extract all reflectors, calculating the pose of the reflectors in the LiDAR coordinate system, and finally converting it to the base coordinate system to obtain the accurate pose.

[0012] As a preferred technical solution of the present invention, in step S1, a base coordinate system T of the mobile robot is defined. baselink , the laser radar coordinate system T laserlink , and there is a transformation matrix between the two The laser radar coordinate system can be converted to the base coordinate system. The reflector specification is L*Wcm and the quantity is N. reflect .

[0013] As a preferred technical solution of the present invention, in step S2, after the reflector is fixed, the laser radar is used to measure the different intensity values ​​returned at a specific distance, and the laser radar angle resolution Angle_Increment, the amount of point cloud data contained in each frame Num_Points, and the specific distance value input_array{r1,r2,...r i}, the intensity value corresponding to a specific distance output_array{Echo1,Echo2,...Echo i}, intensity threshold reflect_score, distance threshold TH dis .

[0014] As a preferred technical solution of the present invention, in step S3, the original laser radar data is received, its distance value and intensity value Point_range, Point_echo are saved, the point cloud data Num_Points are traversed, and the points outside the distance threshold are filtered out. The interpolation algorithm is used to calculate the intensity value of the returned distance value when the reflector exists, and the intensity value and threshold saved in the original data are judged. When the intensity value is greater than the threshold, it is considered to be the value in the reflector, otherwise, it is considered to be an ordinary point; the calculation formula of the intensity value is as follows:

[0015]

[0016] Where output_echo represents the calculated intensity value, input_value represents the input distance value, input_higher_bnd and input_higher_bnd represent the distance interval in which the input distance value is located, and output_array represents the true value of the intensity value measured in the distance interval.

[0017] As a preferred technical solution of the present invention, step S3 also includes recording all strong reflection values, processing the data using a mean filter algorithm, filtering out noise points in the strong reflection values, and filling in invalid points between consecutive strong reflection values ​​to prevent the data from being extracted into two reflectors. The calculation formula is:

[0018] Point_range[i+1]=(Point_range[i]+Point_range[i+2]) / 2

[0019] Point_echo[i+1]=Point_echo[i]

[0020] Where Point_echo[i] represents the intensity value stored at the i-th point in the point cloud data, and its value must satisfy both Point_echo[i]>output_echo[i] and Point_echo[i+2]>output_echo[i].

[0021] As a preferred technical solution of the present invention, step S3 also includes that after preprocessing the obtained raw data, all strong reflection values ​​need to be classified into the correct reflector. Due to the viewing angle range of the laser radar, there may be a situation where the first strong reflection value and the last strong reflection value hit the same reflector, resulting in incorrect division of the reflector. Therefore, it is necessary to process the strong reflection points according to the situation to obtain the correct starting and ending positions of each reflector.

[0022]

[0023]

[0024] Among them, reflector_start_index represents the starting index of the strong reflection point in the same reflector, reflector_end_index represents the ending index of the strong reflection point in the same reflector, and strong_echo_point_index_index represents the index of all strong reflection points.

[0025] As a preferred technical solution of the present invention, step S3 further includes, after completing the statistical classification of the strong reflection values, traversing each reflector to calculate its average distance and maximum reflection value; the calculation formulas are:

[0026]

[0027] max_echo=max(max_echo,Point_echo[i])

[0028] Among them, range represents the average distance of all points in the current reflector, and max_echo represents the maximum intensity value of all points in the current reflector. Only when the maximum intensity value is greater than the set threshold, it is counted in the number of reflectors finally identified; finally, the center of gravity angle of the reflector is calculated; the calculation formula is:

[0029]

[0030] base_angle_rad=angle_start_rad+Angle_Increment*start_index_array[i]

[0031] echo_magnitude_sum=∑Point_echo[i]

[0032] center_angle_rad=base_angle_rad+angle_shift

[0033] Among them, angle_start_rad represents the starting angle in each frame of lidar data, and start_index_array represents the starting index of the midpoint of the reflector.

[0034] As a preferred technical solution of the present invention, step S3 also includes traversing the last number of reflectors, calculating the position of the reflector in the laser radar coordinate system according to the distance value and the angle value, and then converting the position of the reflector to the base coordinate system through the conversion relationship between the coordinate systems; the calculation formula is:

[0035]

[0036] (III) Beneficial effects

[0037] The present invention provides an identification and positioning algorithm based on a single-line laser radar and a reflector. It has the following beneficial effects:

[0038] The present invention combines the reflector information to improve the positioning accuracy of the single-line laser radar in complex environments; the arrangement of the reflectors can be adjusted according to the complexity of the environment to enhance the adaptability of the algorithm; the single-line laser radar has a small amount of data and can quickly give positioning results, and the algorithm has good real-time performance; the high-reflection information of the reflector has strong anti-interference ability, and the positioning result is more stable. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

[0040] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments 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.

[0041] See also Figure 1 , the embodiment of the present invention provides a technical solution: an identification and positioning algorithm based on a single-line laser radar and a reflector, the specific implementation method of which is as follows: first, define the coordinate system of the mobile robot and the laser radar, as well as the size and installation position of the reflector; measure and determine the intensity information returned by the laser radar in the reflector at a specific distance, and set the intensity information threshold and the distance threshold; receive the original point cloud of the laser radar, use linear interpolation to extract all reflectors, calculate the position and posture of the reflector in the laser radar coordinate system, and finally convert it to the base coordinate system to obtain an accurate position and posture. The specific algorithm implementation process is as follows Figure 1 shown.

[0042] (1) Define the base coordinate system T of the mobile robot baselink , the laser radar coordinate system T laserlink , and there is a transformation matrix between the two The laser radar coordinate system can be converted to the base coordinate system. The reflector specification is L*Wcm and the quantity is N. reflect .

[0043] (2) After the reflector is fixed, the laser radar is used to measure the different intensity values ​​returned at a specific distance. The angle resolution of the laser radar is Angle_Increment, the number of point cloud data contained in each frame is Num_Points, and the specific distance value is input_array{r1,r2,...r i}, the intensity value corresponding to a specific distance output_array{Echo1,Echo2,...Echo i}, intensity threshold reflect_score, distance threshold TH dis .

[0044] (3) Receive the original laser radar data, save its distance value and intensity value Point_range, Point_echo, traverse the point cloud data Num_Points, filter out the points outside the distance threshold, use the interpolation algorithm to calculate the intensity value of the returned distance value when the reflector exists, and judge the intensity value and threshold saved in the original data. When the intensity value is greater than the threshold, it is considered to be the value in the reflector, otherwise, it is considered to be an ordinary point; the calculation formula of the intensity value is as follows:

[0045]

[0046] input_higher_bnd=input_array[i+1]

[0047] input_lower_bnd=input_array[i]

[0048] Where output_echo represents the calculated intensity value, input_value represents the input distance value, input_higher_bnd and input_higher_bnd represent the distance interval in which the input distance value is located, and output_array represents the true value of the intensity value measured in the distance interval.

[0049] (4) Record all strong reflection values, use the mean filter algorithm to process the data, filter out the noise points in the strong reflection values, and fill in the invalid points between consecutive strong reflection values ​​to prevent them from being extracted as data from two reflectors. The calculation formula is:

[0050] Point_range[i+1]=(Point_range[i]+Point_range[i+2]) / 2

[0051] Point_echo[i+1]=Point_echo[i]

[0052] Where Point_echo[i] represents the intensity value stored at the i-th point in the point cloud data, and its value must satisfy both Point_echo[i]>output_echo[i] and Point_echo[i+2]>output_echo[i].

[0053] (5) After preprocessing the obtained raw data, all strong reflection values ​​need to be classified into the correct reflector. Due to the viewing angle of the laser radar, there may be a situation where the first strong reflection value and the last strong reflection value hit the same reflector, resulting in incorrect classification of the reflectors. Therefore, it is necessary to process the strong reflection points according to the situation to obtain the correct starting and ending positions of each reflector.

[0054]

[0055] Among them, reflector_start_index represents the starting index of the strong reflection point in the same reflector, reflector_end_index represents the ending index of the strong reflection point in the same reflector, and strong_echo_point_index_index represents the index of all strong reflection points.

[0056] (6) After the strong reflection values ​​are statistically classified, each reflector is traversed to calculate its average distance and maximum reflection value; the calculation formulas are:

[0057]

[0058] max_echo=max(max_echo,Point_echo[i])

[0059] Among them, range represents the average distance of all points in the current reflector, and max_echo represents the maximum intensity value of all points in the current reflector. Only when the maximum intensity value is greater than the set threshold, it is counted in the number of reflectors finally identified; finally, the center of gravity angle of the reflector is calculated; the calculation formula is:

[0060]

[0061] base_angle_rad=angle_start_rad+Angle_Increment*start_index_array[i]

[0062] echo_magnitude_sum=∑Point_echo[i]

[0063] center_angle_rad=base_angle_rad+angle_shift

[0064] Among them, angle_start_rad represents the starting angle in each frame of lidar data, and start_index_array represents the starting index of the midpoint of the reflector.

[0065] (7) Traverse the last number of reflectors, calculate the position of the reflector in the laser radar coordinate system according to the distance value and angle value, and then convert the position of the reflector to the base coordinate system through the conversion relationship between the coordinate systems; the calculation formula is:

[0066]

[0067] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. It is obvious to those skilled in the art that the present invention is not limited to the details of the above exemplary embodiments, and the present invention can be implemented in other specific forms without departing from the spirit or basic features of the present invention. Therefore, no matter from which point of view, the embodiments should be regarded as exemplary and non-restrictive. The scope of the present invention is defined by the attached claims rather than the above description, and it is intended that all changes falling within the meaning and scope of the equivalent elements of the claims are included in the present invention. Any figure mark in the claims should not be regarded as limiting the claims involved.

[0068] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. An identification and positioning algorithm based on a single-line laser radar and a reflector, characterized in that: The following steps are involved: S1. First, define the coordinate system of the mobile robot and the laser radar as well as the size and installation position of the reflector; S2, measuring and determining the intensity information returned by the laser radar in the reflector at a specific distance, and setting the intensity information threshold and the distance threshold; S3, receiving the original point cloud of the LiDAR, using linear interpolation to extract all reflectors, calculating the pose of the reflectors in the LiDAR coordinate system, and finally converting it to the base coordinate system to obtain the accurate pose.

2. The identification and positioning algorithm based on a single-line laser radar and a reflector according to claim 1, characterized in that: In step S1, define the base coordinate system T of the mobile robot baselink , the laser radar coordinate system T laserlink , and there is a transformation matrix between the two The laser radar coordinate system can be converted to the base coordinate system. The reflector specification is L*Wcm and the quantity is N. reflect .

3. The identification and positioning algorithm based on a single-line laser radar and a reflector according to claim 2, characterized in that: In step S2, after the reflector is fixed, the laser radar is used to measure the different intensity values ​​returned at a specific distance. The angle resolution of the laser radar Angle_Increment, the amount of point cloud data contained in each frame Num_Points, and the specific distance value input_array{r1,r2,...r i }, the intensity value corresponding to a specific distance output_array{Echo1,Echo2,...Echo i }, intensity threshold reflect_score, distance threshold TH dis .

4. The identification and positioning algorithm based on a single-line laser radar and a reflector according to claim 3 is characterized in that: In step S3, the original laser radar data is received, and its distance value and intensity value Point_range, Point_echo are saved. The point cloud data Num_Points is traversed, and the points outside the distance threshold are filtered out. The interpolation algorithm is used to calculate the intensity value of any distance value returned when the reflector exists, and the intensity value and threshold saved in the original data are judged. When the intensity value is greater than the threshold, it is considered to be the value in the reflector, otherwise, it is considered to be an ordinary point. The calculation formula of the intensity value is as follows: input_higher_bnd=input_array[i+1] input_lower_bnd=input_array[i] Where output_echo represents the calculated intensity value, input_value represents the input distance value, input_higher_bnd and input_higher_bnd represent the distance interval in which the input distance value is located, and output_array represents the true value of the intensity value measured in the distance interval.

5. The identification and positioning algorithm based on a single-line laser radar and a reflector according to claim 4, characterized in that: Step S3 also includes recording all the strong reflection values, processing the data using a mean filter algorithm, filtering out noise points in the strong reflection values, and filling in invalid points between consecutive strong reflection values ​​to prevent the data from being extracted into two reflectors. The calculation formula is: Point_range[i+1]=(Point_range[i]+Point_range[i+2]) / 2 Point_echo[i+1]=Point_echo[i] Where Point_echo[i] represents the intensity value stored at the i-th point in the point cloud data, and its value must satisfy both Point_echo[i]>output_echo[i] and Point_echo[i+2]>output_echo[i].

6. The identification and positioning algorithm based on a single-line laser radar and a reflector according to claim 5, characterized in that: Step S3 also includes that after preprocessing the obtained raw data, all strong reflection values ​​need to be classified into the correct reflector. Due to the viewing angle range of the laser radar, there may be a situation where the first strong reflection value and the last strong reflection value hit the same reflector, resulting in incorrect division of the reflector. Therefore, it is necessary to process the strong reflection points according to the situation to obtain the correct starting and ending positions of each reflector; according to whether the first strong reflection value and the last strong reflection value hit the same reflector, the start and end indexes of the first reflector to be processed are obtained, which are expressed as: Among them, reflector_start_index represents the starting index of the strong reflection point in the same reflector, reflector_end_index represents the ending index of the strong reflection point in the same reflector, and strong_echo_point_index_index represents the index of all strong reflection points.

7. The identification and positioning algorithm based on a single-line laser radar and a reflector according to claim 6, characterized in that: Step S3 also includes traversing each reflector plate to calculate the average distance and the maximum reflection value after the strong reflection values ​​are statistically classified; the calculation formulas are: max_echo=max(max_echo,Point_echo[i]) Among them, range represents the average distance of all points in the current reflector, and max_echo represents the maximum intensity value of all points in the current reflector. Only when the maximum intensity value is greater than the set threshold, it is counted in the number of reflectors finally identified; finally, the center of gravity angle of the reflector is calculated, and the calculation formula is: angle_weighed_sum=∑Point_echo[i]*(angle_start_rad+Angle_Increment*i-base_angle_rad) base_angle_rad=angle_start_rad+Angle_Increment*start_index_array[i] echo_magnitude_sum=∑Point_echo[i] center_angle_rad=base_angle_rad+angle_shift Among them, angle_start_rad represents the starting angle in each frame of lidar data, and start_index_array represents the starting index of the midpoint of the reflector.

8. The identification and positioning algorithm based on single-line laser radar and reflector according to claim 7 is characterized in that : Step S3 also includes traversing the last number of reflectors, calculating the position of the reflector in the laser radar coordinate system according to the distance value and the angle value, and then converting the position of the reflector to the base coordinate system through the conversion relationship between the coordinate systems; The calculation formula is:

Images