Method and robot for fitting a straight line based on laser point cloud and selecting a reference edge

Abstract

The present invention discloses a method for a robot to fit a straight line based on laser point cloud and select a reference edge, and a robot. When the robot performs a leveling process, it acquires the initial pose information of the robot and the first set of laser point cloud data; the robot rotates by a first preset angle to acquire the reference pose information of the robot and the second set of laser point cloud data; performs coordinate transformation processing on the initial pose information of the robot and the first set of laser point cloud data to acquire three sets of coordinate information; calculates the angle and the obstacle distance according to the reference pose information of the robot and the three sets of coordinate information to acquire the third set of laser point cloud data; performs mean value calculation on the second set of laser point cloud data and the third set of laser point cloud data and substitutes them into the least squares method to acquire N fitted straight lines; the present invention uses a method of fusing laser point cloud data in different poses to eliminate the influence of sudden situations such as environmental occlusion and positioning deviation on the detection of laser point cloud data, making the fitted straight line straighter and more reliable, and improving the mapping quality of the robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning and navigation, and specifically relates to a method for fitting a straight line based on laser point cloud and selecting a reference edge, and a robot. Background Art

[0002] With the rapid development of science and technology, the cost of lidar, which was often used in the military field in the past, has been greatly reduced, resulting in a sharp increase in the application of lidar in the commercial field. Lidar has the advantages of good monochromaticity, high brightness, strong directivity, strong anti-interference ability, strong resolution, and small and light equipment, and has a wide range of applications in the field of robot positioning. With the development of robot technology, various types of robots have been widely used in various fields of daily life, and robot positioning and navigation technology is one of the most core technical fields of robots. Traditional robots generally use lidar sensors for positioning and navigation. During the scanning process of the lidar, due to the influence of obstacles in the environment, the laser point cloud data obtained by the robot may be distorted to a certain extent, and the laser point cloud data obtained by the robot through a single pose may be affected by obstacles and unable to fit a straight line, affecting the mapping quality of the robot. Summary of the Invention

[0003] To solve the above problems, the present invention provides a method for a robot to fit a straight line based on laser point cloud and select a reference edge, and a robot. By collecting laser point cloud data of the robot in different poses, the "field of view" obtained by the robot is wider, the laser point cloud data collected in multiple different poses is more stable after fusion, the fitted straight line is straighter, the selected reference edge is more suitable for the current environment, and the mapping quality of the robot is improved. The specific technical solutions of the present invention are as follows:

[0004] Method for fitting a straight line based on lidar point cloud. The method specifically includes: Step S1: When the robot performs a leveling process, obtain the initial pose information of the robot based on the lidar mounted on the robot, and collect the first set of lidar point cloud data; Step S2: Control the robot to rotate a first preset angle, obtain the reference pose information of the robot based on the lidar mounted on the robot, and collect the second set of lidar point cloud data; Step S3: Perform coordinate transformation processing according to the initial pose information of the robot and the first set of lidar point cloud data obtained in Step S1 to obtain three sets of coordinate information; Step S4: Calculate the angle and obstacle distance based on the reference pose information of the robot obtained in Step S2 and the three sets of coordinate information obtained in Step S3 to obtain the third set of lidar point cloud data; Step S5: Perform mean calculation according to the second set of lidar point cloud data obtained in Step S2 and the third set of lidar point cloud data obtained in Step S4, and obtain N fitted straight lines based on the least squares method; where the leveling process refers to the process of the robot finding parallel walls during mapping; each set of lidar point cloud data includes three frames of lidar point cloud data; the lidar point cloud data specifically includes the distance R between the lidar and the obstacle and the angle θ between the lidar and the obstacle; the number N of the fitted straight lines is an integer greater than or equal to 1. This method weakens the influence of uncertain factors in the environment that cause distortion of the lidar point cloud data by fusing the lidar point cloud data of the robot in different poses, and eliminates the influence of sudden situations such as environmental changes, occlusion, and positioning deviation on the detection of the lidar point cloud data. It improves the straightness of the fitted straight line obtained from the lidar point cloud data and improves the mapping quality of the robot.

[0005] Further, the initial pose information of the robot specifically includes the x-axis coordinate x0 of the initial pose of the robot, the y-axis coordinate y0 of the initial pose of the robot, and the initial angle β0 between the lidar and the obstacle; the first set of lidar point cloud data includes the first frame of the first set of lidar point cloud data, the second frame of the first set of lidar point cloud data, and the third frame of the first set of lidar point cloud data. The first frame of the first set of lidar point cloud data includes the first distance R between the lidar and the obstacle 1j and the first angle θ between the lidar and the obstacle 1j , the second frame of the first set of lidar point cloud data includes the second distance R between the lidar and the obstacle 2j and the second angle θ between the lidar and the obstacle 2j , the third frame of the first set of lidar point cloud data includes the third distance R between the lidar and the obstacle 3j and the third angle θ between the lidar and the obstacle 3j ; where the coordinates in the initial position information of the robot use the robot coordinate system.

[0006] Further, the robot reference pose information specifically includes the x-axis coordinate x1 of the robot reference pose, the y-axis coordinate y1 of the robot reference pose, and the reference angle β1 between the lidar and the obstacle; the second set of lidar point cloud data includes the first frame of the second set of lidar point cloud data, the second frame of the second set of lidar point cloud data, and the third frame of the second set of lidar point cloud data. The first frame of the second set of lidar point cloud data includes the fourth distance R between the lidar and the obstacle 4j and the fourth angle θ between the lidar and the obstacle 4j , the second frame of the second set of lidar point cloud data includes the fifth distance R between the lidar and the obstacle 5j and the fifth angle θ between the lidar and the obstacle 5j , the third frame of the second set of lidar point cloud data includes the sixth distance R between the lidar and the obstacle 6j and the sixth angle θ between the lidar and the obstacle 6j . This method obtains two sets of lidar point cloud data by acquiring the initial pose information and reference pose information of the robot. The fusion of multiple sets of lidar point cloud data can make the fitted line straighter and more reliable.

[0007] Further, the step S3 specifically includes: substituting the x-axis coordinate x0 of the robot initial pose, the y-axis coordinate y0 of the robot initial pose, and the initial angle β0 between the lidar of the robot and the obstacle in the robot initial pose information obtained in step S1 into the coordinate transformation formula in the form of parameters; substituting the three frames of lidar point cloud data in the first set of lidar point cloud data obtained in step S1 into the coordinate transformation formula in the form of parameters to obtain three sets of coordinate information; wherein, the coordinate transformation refers to the transformation from the robot coordinate system to the world coordinate system; the coordinate information includes the x-axis coordinate in the world coordinate system and the y-axis coordinate in the world coordinate system; the three sets of coordinate information specifically include the first set of coordinate information obtained by substituting the first frame of the first set of lidar point cloud data into the coordinate transformation formula, the second set of coordinate information obtained by substituting the second frame of the first set of lidar point cloud data into the coordinate transformation formula, and the third set of coordinate information obtained by substituting the third frame of the first set of lidar point cloud data into the coordinate transformation formula; the first set of coordinate information includes the first x-axis coordinate x N1j and the first y-axis coordinate y N1j , the second set of coordinate information includes the second x-axis coordinate x N2j and the second y-axis coordinate y N2j , the third set of coordinate information includes the third x-axis coordinate x N3j and the third y-axis coordinate y N3j . This method combines the initial pose information of the robot and the first set of lidar point cloud data to convert from the robot coordinate system to the world coordinate system, so as to determine the coordinate position of the robot in different scenarios.

[0008] Further, the coordinate transformation formula for transforming the robot coordinate system into the world coordinate system is as follows:

[0009]

[0010] where y Nij refers to the y-axis coordinate of the robot in the world coordinate system, x Nij refers to the x-axis coordinate of the robot in the world coordinate system, R ij refers to the distance between the lidar and the obstacle, θ ij refers to the angle between the lidar and the obstacle; i is the subscript number used to distinguish the group or frame number to which the parameter belongs, i is an integer greater than or equal to 1 and less than or equal to 3, and j is the subscript number used to distinguish the parameters belonging to the same group or the same frame number, j is an integer greater than or equal to 0 and less than or equal to 359.

[0011] Further, the method for obtaining the third set of lidar point cloud data in step S4 specifically includes: substituting the x-axis coordinate x1 and the y-axis coordinate y1 of the robot reference pose information obtained in step S2 into the coordinate difference formula in the form of parameters; substituting the three sets of coordinate information obtained in step S3 into the coordinate difference formula in the form of x-axis coordinate parameters and y-axis coordinate parameters respectively to obtain three sets of coordinate differences; substituting the three sets of obtained coordinate differences into the angle calculation formula and the obstacle distance calculation formula in the form of parameters to obtain the third set of lidar point cloud data; where the three sets of coordinate differences are respectively, the first set of coordinate differences includes the first x-axis coordinate difference dis_x N1j and the first y-axis coordinate difference dis_y N1j , the second set of coordinate differences includes the second x-axis coordinate difference dis_x N2j and the second y-axis coordinate difference dis_y N2j , the third set of coordinate differences includes the third x-axis coordinate difference dis_x N3j and the third y-axis coordinate difference dis_y N3j ; the third set of lidar point cloud data includes three frames of lidar point cloud data, the first frame of the third set of lidar point cloud data includes the seventh distance R 7j between the lidar and the obstacle and the seventh angle θ 7j between the lidar and the obstacle, the second frame of the third set of lidar point cloud data includes the eighth distance R 8j between the lidar and the obstacle and the eighth angle θ 8j between the lidar and the obstacle, the third frame of the third set of lidar point cloud data includes the ninth distance R 9j between the lidar and the obstacle and the ninth angle θ 9j .

[0012] Furthermore, the coordinate difference formula is as follows:

[0013]

[0014] The angle calculation formula is as follows:

[0015]

[0016] The obstacle distance calculation formula is as follows:

[0017]

[0018] Among them, the fabs function is used to calculate the absolute value; the fmod function is used to calculate the remainder of a floating-point number; the atan2 function is used to calculate the included angle; the sqrt function is used to calculate the square root; the pow function is used to calculate the power; i is the subscript number used to distinguish the group or frame number to which the parameter belongs, i is an integer greater than or equal to 1 and less than or equal to 3, and j is the subscript number used to distinguish the parameters belonging to the same group or the same frame number, j is an integer greater than or equal to 0 and less than or equal to 359.

[0019] Furthermore, step S5 specifically includes: averaging a total of six frames of data, namely the second group of lidar point cloud data obtained in step S2 and the third group of lidar point cloud data obtained in step S5, to obtain the average lidar point cloud data T0; substituting the average lidar point cloud data T0 into the least squares formula in the form of a parameter to obtain the optimal solution of the fitted straight line; among them, the average lidar point cloud data T0 includes the average angle between the lidar and the obstacle and the average distance between the lidar and the obstacle. The method of fusing lidar point cloud data in different poses of the robot is used to eliminate the influence of sudden situations such as environmental changes, occlusions, and positioning deviations on the detection of lidar point cloud data.

[0020] The present invention also discloses a method for selecting a reference edge based on lidar point cloud. The method for selecting a reference edge based on lidar point cloud is implemented on the basis of the aforementioned method for fitting a straight line based on lidar point cloud. When N fitted straight lines are obtained by the method for fitting a straight line based on lidar point cloud as described above and the value of N is an integer greater than or equal to 2, the fitted straight line corresponding to the maximum value among the total lengths of the N fitted straight lines is selected as the best parallel wall reference edge; among them, each fitted straight line has a corresponding total length of the fitted straight line.

[0021] Further, the step of selecting the fitting line corresponding to the maximum value among the total lengths of the N fitting lines as the best parallel wall reference edge specifically includes the following steps: Step S601: According to the N fitting lines obtained in Step S5, select one of the fitting lines as the current reference edge, and obtain the length of the current reference edge, then enter Step S602; Step S602: Project the remaining N - 1 fitting lines in the direction of the current reference edge to obtain N - 1 first projection values corresponding to the remaining N - 1 fitting lines, and project the remaining N - 1 fitting lines in the direction perpendicular to the current reference edge to obtain N - 1 second projection values corresponding to the remaining N - 1 fitting lines, then enter Step S603; Step S603: Take the product of the larger value of the first projection value and the second projection value corresponding to the same fitting line and the first preset parameter b as the third projection value corresponding to this fitting line, and obtain N - 1 third projection values corresponding to the remaining N - 1 fitting lines, then enter Step S604; Step S604: Calculate the sum of the length of the current reference edge and the N - 1 third projection values corresponding to the remaining N - 1 fitting lines, and take the sum of the length of the current reference edge and the N - 1 third projection values corresponding to the remaining N - 1 fitting lines as the total length of the fitting line corresponding to the current reference edge for this fitting line, then enter Step S605; Step S605: Repeat the above Steps S601 to S605 until all N fitting lines are traversed to obtain N total lengths of the fitting lines corresponding to the N fitting lines, then enter Step S606; Step S606: Select the fitting line corresponding to the maximum value among the N total lengths of the fitting lines as the best parallel wall reference edge; wherein, the number N of the fitting lines is an integer greater than or equal to 2. Compared with the prior art, the robot of the present technical solution uses multiple sets of laser point cloud data obtained by itself in different poses to perform fusion transformation operations to achieve the effect of broadening the robot's field of view, and then obtains N fitting lines based on the fusion transformation operation results of the multiple sets of laser point cloud data. By comparing and operating on the N fitting lines, the optimal fitting line that conforms to the current environment is selected as the best parallel wall reference edge, ensuring that the robot obtains a straight reference edge, improving the mapping quality of the robot, and this technical solution can adjust the specific conditions and operation parameters for selecting the reference edge according to the actual environment where the robot is located, so as to achieve the purpose of flexibly selecting the most suitable fitting line as the reference edge for the current environment.

[0022] The present invention also discloses a robot, including: one or more processors, one or more chips, and one or more computer programs stored on the chips and executable on the processors. When the processors execute the computer programs, the steps included in the above method are implemented.

[0023] The present invention also discloses a chip, where the chip stores a computer program, and when the computer program is run by a processor, the steps of any of the foregoing methods are implemented. Brief Description of the Drawings

[0024] Figure 1 FIG. is a schematic flowchart of a method for a robot to fit a straight line based on laser point cloud according to an embodiment of the present invention.

[0025] Figure 2 FIG. is a schematic flowchart of a method for a robot to select a reference edge based on laser point cloud according to an embodiment of the present invention. Detailed Embodiments

[0026] In order to make the objectives, technical solutions and advantages of the present application more clear and understandable, the present application will be described and explained below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application. All other embodiments obtained by those of ordinary skill in the art based on the embodiments provided in the present application without making creative efforts fall within the scope of protection of the present application.

[0027] Obviously, the drawings described below are only some examples or embodiments of the present application. For those of ordinary skill in the art, without making creative efforts, the present application can also be applied to other similar scenarios based on these drawings. In addition, it can also be understood that although the efforts made in this development process may be complex and lengthy, for those of ordinary skill in the art related to the content disclosed in the present application, some design, manufacturing or production changes made based on the technical content disclosed in the present application are only conventional technical means and should not be understood as insufficient disclosure of the present application.

[0028] Referring to "embodiment" in the present application means that the specific features, structures or characteristics described in connection with the embodiment may be included in at least one embodiment of the present application. The phrase appears at various positions in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those of ordinary skill in the art will explicitly or implicitly understand that the embodiments described in the present application can be combined with other embodiments without conflict.

[0029] Unless otherwise defined, the technical terms or scientific terms involved in this application shall have the ordinary meanings understood by those with ordinary skills in the technical field to which this application pertains. The words such as "a", "an", "one", "the" and the like involved in this application do not indicate a quantity limitation and may represent a singular or plural number. The terms "comprising", "including", "having" and any variations thereof involved in this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device comprising a series of steps or modules is not limited to the listed steps or units, but may further include steps or units not listed, or may further include other steps or units inherent to these processes, methods, products or devices. The terms "first", "second", "third" involved in this application are only used to distinguish similar objects and do not represent a specific order for the objects.

[0030] The method for a robot to fit a straight line based on laser point cloud provided in this application is applied to robot positioning and navigation. A lidar device is mounted on the robot body. It should be noted that the lidar device emits a laser beam towards the target, compares and processes the emitted signal with the received reflected signal, and obtains parameter information such as the distance, attitude, height, and azimuth of the target.

[0031] In an embodiment of the present invention, a method for a robot to fit a straight line based on laser point cloud is provided. Figure 1 It is a flowchart of the method for a robot to fit a straight line based on laser point cloud according to an embodiment of the present invention. Refer to Figure 1 As shown, the method specifically includes the following steps:

[0032] Step S1: When the robot performs the leveling process, it obtains the initial pose information of the robot based on the lidar mounted on the robot and collects the first set of laser point cloud data; wherein, the laser point cloud data is obtained by the lidar, and the initial pose information of the robot is calculated and obtained by using the Cartogarphy system.

[0033] It should be noted that in this embodiment and the following embodiments, the lidar is composed of a transmitting system, a receiving system, an information processing system, etc. It uses a laser beam to densely sample the application environment of the robot to obtain high-precision laser point cloud data. Each set of laser point cloud data contains three frames of laser point cloud data. The first set of laser point cloud data includes the first set of the first frame of laser point cloud data, the first set of the second frame of laser point cloud data, and the first set of the third frame of laser point cloud data. The first set of the first frame of laser point cloud data includes the first distance R between the lidar and the obstacle 1j and the first angle θ between the lidar and the obstacle 1j , the first set of the second frame of laser point cloud data includes the second distance R between the lidar and the obstacle 2jand the second angle θ between the lidar and the obstacle 2j , the first set of the third-frame lidar point cloud data includes the third distance R between the lidar and the obstacle 3j and the third angle θ between the lidar and the obstacle 3j . Specifically, each frame of the lidar point cloud data in a set of lidar point cloud data includes 360 distance information between the lidar and the obstacle and 360 angle information between the lidar and the obstacle obtained by the lidar from 0 to 359 degrees; the subscript j in the distance information and angle information between the lidar and the obstacle represents the angle of the lidar in this frame of lidar point cloud data, and j is an integer greater than or equal to 0 and less than or equal to 359.

[0034] It should be noted that the leveling process refers to the process of the robot finding a flat surface or edge as a parallel wall when mapping, and the flat surface or edge can be a wall surface or the edge of a flat object such as a cabinet or a desk; the lidar point cloud data specifically includes the distance R between the lidar and the obstacle and the angle θ between the lidar and the obstacle.

[0035] The initial pose information P0 of the robot calculated and obtained by the Cartography system includes the x-axis coordinate x0 of the robot's initial pose, the y-axis coordinate y0 of the robot's initial pose, and the initial angle β0 between the lidar and the obstacle. It should be noted that the coordinates in the robot pose information calculated and obtained by the Cartography system belong to the robot coordinate system.

[0036] Step S2: Control the robot to rotate a first preset angle, obtain the robot reference pose information based on the lidar mounted on the robot, and collect a second set of lidar point cloud data; where the first preset angle refers to the preset rotation angle of the robot. It should be noted that the specific rotation angle value of the robot can be adaptively adjusted according to the robot application scenario.

[0037] Specifically, the robot reference pose information specifically includes the x-axis coordinate x1 of the robot reference pose, the y-axis coordinate y1 of the robot reference pose, and the reference angle β1 between the lidar and the obstacle; obtaining the robot reference pose information and the robot initial pose information is to obtain the lidar point cloud data of the robot in different poses in the same application scenario. The second set of lidar point cloud data includes the first frame of the second set of lidar point cloud data, the second frame of the second set of lidar point cloud data, and the third frame of the second set of lidar point cloud data. The first frame of the second set of lidar point cloud data includes the fourth distance R between the lidar and the obstacle 4j and the fourth angle θ between the lidar and the obstacle 4j , the second frame of the second set of lidar point cloud data includes the fifth distance R between the lidar and the obstacle 5j and the fifth angle θ between the lidar and the obstacle 5j, the third frame of the second group of lidar point cloud data includes the sixth distance R between the lidar and the obstacle 6j and the sixth angle θ between the lidar and the obstacle 6j . Specifically, each frame of the lidar point cloud data in the second group of lidar point cloud data includes 360 distance information between the lidar and the obstacle and 360 angle information between the lidar and the obstacle obtained by the lidar from 0 to 359 degrees; the subscript j in the distance information and angle information between the lidar and the obstacle represents the angle of the lidar in this frame of lidar point cloud data, and j is an integer greater than or equal to 0 and less than or equal to 359.

[0038] Step S3: Perform coordinate transformation processing on the initial pose information P0 of the robot according to the first group of lidar point cloud data obtained in Step S1 and the coordinate transformation formula to obtain three groups of coordinate information.

[0039] Specifically, the coordinate transformation refers to converting the robot coordinate system into the world coordinate system; the world coordinate system refers to the rectangular coordinate system with reference to the earth; the coordinate transformation formula is:

[0040]

[0041] In the above coordinate transformation formula, R i refers to the first distance R1, the second distance R2, or the third distance R3 between the lidar and the obstacle in three frames of lidar point cloud data in the first group of lidar point cloud data; θ i refers to the first angle θ1, the second angle θ2, or the third angle θ3 between the lidar and the obstacle in three frames of lidar point cloud data in the first group of lidar point cloud data; x0 refers to the x-axis coordinate of the robot's initial pose in the robot coordinate system; y0 refers to the y-axis coordinate of the robot's initial pose in the robot coordinate system; β0 refers to the initial angle between the lidar of the robot and the obstacle in the initial pose of the robot; x Nij refers to the x-axis coordinate of the robot in the world coordinate system; y Nij refers to the y-axis coordinate of the robot in the world coordinate system; i is the subscript serial number used to distinguish the group or frame to which the parameter belongs, and i is an integer greater than or equal to 1 and less than or equal to 3, and j is the subscript serial number used to distinguish the parameters belonging to the same group or the same frame, and j is an integer greater than or equal to 0 and less than or equal to 359; when the coordinate transformation formula is operated, it will run according to the rule of changing the i value once every time the j value is traversed, that is, after the j value traverses from 0 to 359, the i value is changed, such as changing from the i value 1 to 2 or the i value changing from 2 to 3, until all i values are traversed, and the operation of the coordinate transformation formula ends.

[0042] Specifically, the three sets of coordinate information specifically include the first set of coordinate information obtained by substituting the first-frame laser point cloud data of the first group into the coordinate transformation formula, the second set of coordinate information obtained by substituting the second-frame laser point cloud data of the first group into the coordinate transformation formula, and the third set of coordinate information obtained by substituting the third-frame laser point cloud data of the first group into the coordinate transformation formula; the first set of coordinate information includes the first x-axis coordinate x N1j and the first y-axis coordinate y N1j , the second set of coordinate information includes the second x-axis coordinate x N2j and the second y-axis coordinate y N2j , the third set of coordinate information includes the third x-axis coordinate x N3j and the third y-axis coordinate y N3j .

[0043] Step S401: According to the robot reference pose information and the coordinate difference formula, perform difference calculations on the three sets of coordinate information to obtain three sets of coordinate differences.

[0044] Specifically, the difference calculation refers to calculating the differences between the robot reference pose information and the x-axis and y-axis coordinates in the three sets of coordinate information, that is, the distance between the robot reference pose coordinates in the robot reference pose information and the robot coordinates in the three sets of coordinate information; the coordinate difference formula is:

[0045]

[0046] In the above coordinate difference formula, the fabs function is used to find the absolute value to avoid the situation where the coordinate difference is negative; x1 refers to the x-axis coordinate of the robot reference pose in the robot coordinate system; y1 refers to the y-axis coordinate of the robot reference pose in the robot coordinate system; x Nij can be the first x-axis coordinate x N1j in the first set of coordinate information, the second x-axis coordinate x N2j in the second set of coordinate information, or the third x-axis coordinate x N3j in the third set of coordinate information; y Nij can be the first y-axis coordinate y N1j in the first set of coordinate information, the second y-axis coordinate y N2j in the second set of coordinate information, or the third y-axis coordinate y N3j in the third set of coordinate information; dis_y Nij refers to the robot y-axis coordinate difference; dis_x NijIt refers to the difference in the x-axis coordinates of the robot; i is the subscript number used to distinguish the group or frame number to which the parameter belongs. i is an integer greater than or equal to 1 and less than or equal to 3. j is the subscript number used to distinguish the parameters belonging to the same group or the same frame number. j represents the lidar angle corresponding to the parameter in that group or that frame number. j is an integer greater than or equal to 0 and less than or equal to 359. For example, x N1j refers to the first x-axis coordinate corresponding to the lidar from 0 to 359 degrees in the first group of coordinate information. j changes accordingly according to the lidar angle. x N1j includes x N11 、x N12 、x N13 ...x N1359 etc.; When performing the coordinate difference formula operation, it follows the same operation rules as the above coordinate transformation formula. The i value is changed once after each traversal of the j value until all i values are traversed, and then the operation of the coordinate difference formula ends.

[0047] Specifically, the three groups of coordinate differences include: The first group of coordinate differences specifically includes the first x-axis coordinate difference dis_x N1j and the first y-axis coordinate difference dis_y N1j , the second group of coordinate differences specifically includes the second x-axis coordinate difference dis_x N2j and the second y-axis coordinate difference dis_y N2j , the third group of coordinate differences specifically includes the third x-axis coordinate difference dis_x N3j and the third y-axis coordinate difference dis_y N3j .

[0048] Step S402: Substitute the three groups of coordinate differences into the angle calculation formula and the obstacle distance calculation formula in the form of parameters respectively to obtain the third group of lidar point cloud data; among them, the three frames of lidar point cloud data included in the third group of lidar point cloud data are three groups of angle information and three groups of obstacle distance information calculated by substituting the three groups of coordinate differences into the angle calculation formula and the obstacle distance calculation formula respectively.

[0049] Specifically, the angle calculation formula is used to calculate the angle between two coordinate points. The angle calculation formula is:

[0050]

[0051] In the above angle calculation formula, the fmod function is used to find the remainder to obtain an angle value less than 360 degrees; the atan2 function is used to calculate the polar angle formed by the coordinate differences; dis_y Ni refers to the difference in the y-axis coordinates of the robot; dis_x NiIt refers to the difference in the x-axis coordinates of the robot; i is the subscript number used to distinguish the group or frame number to which the parameter belongs, i is an integer greater than or equal to 1 and less than or equal to 3, and j is the subscript number used to distinguish the parameters belonging to the same group or the same frame number, j is an integer greater than or equal to 0 and less than or equal to 359; when calculating the above angle calculation formula, the same operation rules as the above coordinate transformation formula and the above coordinate difference formula are followed. Each time the j value is traversed, the i value is changed once, and the operation of the angle calculation formula ends until all i values are traversed.

[0052] Specifically, the obstacle distance calculation formula is:

[0053]

[0054] In the above obstacle distance calculation formula, the sqrt function is used for square root calculation; the pow function is used for power operation; the obstacle distance calculation formula calculates the obstacle distance information through the Pythagorean theorem of a triangle; i is the subscript number used to distinguish the group or frame number to which the parameter belongs, i is an integer greater than or equal to 1 and less than or equal to 3, and j is the subscript number used to distinguish the parameters belonging to the same group or the same frame number, j is an integer greater than or equal to 0 and less than or equal to 359; when calculating the obstacle distance calculation formula, the same operation rules as the above coordinate transformation formula, the above coordinate difference formula, and the above angle calculation formula are followed. Each time the j value is traversed, the i value is changed once, and the operation of the obstacle distance calculation formula ends until all i values are traversed.

[0055] Specifically, the first group of coordinate differences includes the first x-axis coordinate difference dis_x N1j and the first y-axis coordinate difference dis_y N1j which are substituted into the angle calculation formula and the obstacle distance calculation formula in the form of parameters to obtain the third group of first-frame lidar point cloud data; and so on, the second group of coordinate differences are substituted into the calculation to obtain the third group of second-frame lidar point cloud data, and the third group of coordinate differences are substituted into the calculation to obtain the third group of third-frame lidar point cloud data.

[0056] Among them, the third group of first-frame lidar point cloud data includes the seventh distance R between the lidar and the obstacle 7j and the seventh angle θ between the lidar and the obstacle 7j , the third group of second-frame lidar point cloud data includes the eighth distance R between the lidar and the obstacle 8j and the eighth angle θ between the lidar and the obstacle 8j , the third group of third-frame lidar point cloud data includes the ninth distance R between the lidar and the obstacle 9j and the ninth angle θ between the lidar and the obstacle 9j .

[0057] Step S501: Perform an averaging operation on the second set of lidar point cloud data and the third set of lidar point cloud data to obtain average lidar point cloud data.

[0058] Specifically, a total of six frames of lidar point cloud data from the second set of lidar point cloud data and the third set of lidar point cloud data are subjected to an averaging operation. The averaging operation formula is:

[0059]

[0060] wherein, the average lidar point cloud data includes the average distance between the lidar and the obstacle; for each lidar angle j in each frame of lidar point cloud data, there corresponds a distance R between the lidar and the obstacle ij .

[0061] Step S502: Substitute the average lidar point cloud data obtained in Step S501 into the least squares method operation in the form of parameters to obtain N fitted straight lines.

[0062] Specifically, the obtaining of N fitted straight lines can be, but is not limited to, only using the better data in the calculation of the mean value of the lidar point cloud data to fit a straight line in combination with the least squares method, or using all the lidar point cloud data of a specified group to fit a straight line in combination with the least squares method; the better data in the lidar point cloud data refers to the lidar point cloud data that conforms to a preset better screening rule. The preset better screening rule can be, but is not limited to, a rule for screening out the lidar point cloud data for fitting a smoother and straighter fitted straight line. The better screening rule can be adjusted according to the actual application scenario or the actual model of the application robot; in the method for obtaining a fitted straight line based on lidar point cloud disclosed in this embodiment, N is an integer greater than or equal to 1.

[0063] Through Steps S1 to S502, the lidar point cloud data obtained by the robot in different poses is fused and processed to eliminate possible interference in the environment, calculate the average lidar point cloud data, solve the fitted straight line equation using the least squares method, and can screen the lidar point cloud data by presetting a better screening rule for the lidar point cloud data, select the lidar point cloud data that most conforms to the better screening rule to obtain the fitted straight line, further improving the straightness reliability of the fitted straight line and the quality of the robot's positioning and mapping. The embodiment of the present invention adopts a method of fusing lidar point cloud data in different poses of the robot to eliminate the influence of sudden situations such as environmental changes, occlusions, and positioning deviations on the detection of lidar point cloud data.

[0064] Another embodiment of the present invention discloses a method for a robot to select a reference edge based on laser point cloud on the basis of the method for the robot to obtain a fitted straight line based on laser point cloud described in the foregoing embodiment. That is, on the basis of steps S1 to S502 of the foregoing embodiment, when the N fitted straight lines obtained in step S502 and the value of N is an integer greater than or equal to 2, step S601 is entered. The specific steps of the method for the robot to select a reference edge based on laser point cloud, in addition to steps S1 to S502 in the foregoing embodiment, further include:

[0065] Step S601: According to the optimal solution of the N fitted straight lines obtained in step S502, select one of the fitted straight lines as the current reference edge, and obtain the length l of the current reference edge, and then enter step S602. n , and then enter step S602.

[0066] Step S602: Project the remaining N - 1 fitted straight lines in the direction of the current reference edge to obtain N - 1 first projection values corresponding to the remaining N - 1 fitted straight lines. Project the remaining N - 1 fitted straight lines in the direction perpendicular to the current reference edge to obtain N - 1 second projection values corresponding to the remaining N - 1 fitted straight lines, and then enter step S603. Specifically, by projecting the remaining N - 1 fitted straight lines in the direction of the current reference edge and in the direction perpendicular to the current reference edge, N - 1 first projection values l a1 and N - 1 second projection values corresponding to the remaining N - 1 fitted straight lines are obtained.

[0067] Step S603: Take the product of the larger value of the first projection value and the second projection value corresponding to the same fitted straight line and the first preset parameter b as the third projection value of the fitted straight line, and obtain N - 1 third projection values corresponding to the remaining N - 1 fitted straight lines, and then enter step S604. Among them, the first preset parameter b is a parameter that can be adjusted according to the actual environment and the actual robot model.

[0068] Step S604: Calculate the sum of the length of the current reference edge and the N - 1 third projection values corresponding to the remaining N - 1 fitted straight lines, and take the sum of the length of the current reference edge and the N - 1 third projection values corresponding to the remaining N - 1 fitted straight lines as the total length of the fitted straight line corresponding to the current reference edge for this fitted straight line, and then enter step S605.

[0069] Step S605: Determine whether the N total lengths of the fitted straight lines corresponding to the N fitted straight lines have been obtained. If so, enter step S606. If not, return to step S601. Specifically, by determining whether the N total lengths of the fitted straight lines corresponding to the N fitted straight lines have been obtained, it is achieved to determine whether the robot has traversed all the fitted straight lines, avoiding omission and affecting the selection of the best parallel wall reference edge.

[0070] Step S606: Select the fitting line corresponding to the maximum value among the total lengths of N fitting lines as the best reference edge for parallel walls. Specifically, in the process of selecting the best reference edge for parallel walls, the first preset parameter b used in step S603 and the fitting line corresponding to the maximum value among the total lengths of N fitting lines selected in step S606 can be specifically adjusted according to the actual application scenario of the robot and the actual robot model. For example, modify the value of the first preset parameter b, or modify it to select the minimum value or median value among the total lengths of N fitting lines, so that the method for the robot to select the reference edge based on the laser point cloud can be more flexibly adapted to various scenarios and requirements.

[0071] It should be noted that the steps shown in the above process or the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. And although the logical order is shown in the flowchart, in some cases or scenarios, the steps shown or described can be executed in a different order than here.

[0072] In an embodiment of the present invention, a robot is further provided. The robot is used to implement the above embodiments and preferred implementation manners. The robot includes one or more processors, one or more memories, and one or more computer programs stored on the memories and executable on the processors; when the processor executes the computer program, the method steps described in the foregoing embodiments or preferred implementation manners are implemented.

[0073] In an embodiment of the present invention, a chip is further provided, on which a computer program is stored. When the computer program is executed by a processor, the steps in the method for fitting a straight line to laser point cloud data with different poses of the robot provided in the above various embodiments are implemented.

[0074] Those of ordinary skill in the art can understand that to implement all or part of the processes in the above embodiment methods, it can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above method embodiments. Among them, the references to memories, storage, databases, or other media used in the various embodiments provided in the present application can all include non-volatile and / or volatile memories. Non-volatile memories can include read-only memory ROM, programmable memory PROM, electrically programmable memory EPROM, electrically erasable programmable memory EEPROM, or flash memory. Volatile memories can include random access memory RAM or external cache memory.

[0075] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope recorded in this specification.

[0076] The above embodiments only represent several embodiments of the present invention, and their descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the invention patent. It should be noted that for those of ordinary skill in the art, without departing from the concept of this application, several modifications and improvements can still be made, and these all belong to the protection scope of this application.

Claims

1. A method for fitting a straight line based on laser point cloud, characterized in that, The method specifically includes: Step S1: When the robot performs the leveling process, based on the lidar mounted on the robot, obtain the initial pose information of the robot and collect the first set of lidar point cloud data; Step S2: Control the robot to rotate by a first preset angle, based on the lidar mounted on the robot, obtain the reference pose information of the robot and collect the second set of lidar point cloud data; Step S3: Perform coordinate transformation processing according to the initial pose information of the robot and the first set of lidar point cloud data obtained in Step S1 to obtain three sets of coordinate information; Step S4: Calculate the angle and the distance to the obstacle based on the reference pose information of the robot obtained in Step S2 and the three sets of coordinate information obtained in Step S3 to obtain the third set of lidar point cloud data; Step S5: Perform mean calculation according to the second set of lidar point cloud data obtained in Step S2 and the third set of lidar point cloud data obtained in Step S4, and obtain N fitting lines based on the least squares method; Among them, the leveling process refers to the process of the robot finding a straight edge or surface as a parallel wall when building a map; each set of lidar point cloud data includes three frames of lidar point cloud data; the lidar point cloud data specifically includes the distance R between the lidar and the obstacle and the angle θ between the lidar and the obstacle; the number N of the fitting lines is an integer greater than or equal to 1; Among them, the method for obtaining the third set of lidar point cloud data in Step S4 specifically includes: Substitute the x-axis coordinate (x1) of the robot reference pose and the y-axis coordinate (y1) of the robot reference pose in the robot reference pose information obtained in Step S2 into the coordinate difference formula in the form of parameters; Substitute the three sets of coordinate information obtained in Step S3 into the coordinate difference formula in the form of x-axis coordinate parameters and y-axis coordinate parameters respectively to obtain three sets of coordinate differences; Substitute the three sets of obtained coordinate differences into the angle calculation formula and the obstacle distance calculation formula in the form of parameters respectively to obtain the third set of lidar point cloud data; Among them, the three groups of coordinate differences are as follows. The first group of coordinate differences includes the first x-axis coordinate difference (dis_x N1j ) and the first y-axis coordinate difference (dis_y N1j ). The second group of coordinate differences includes the second x-axis coordinate difference (dis_x N2j ) and the second y-axis coordinate difference (dis_y N2j ). The third group of coordinate differences includes the third x-axis coordinate difference (dis_x N3j ) and the third y-axis coordinate difference (dis_y N3j ); The third group of lidar point cloud data includes three frames of lidar point cloud data. The first frame of the third group of lidar point cloud data includes the seventh distance (R 7j ) between the lidar and the obstacle and the seventh angle (θ 7j ) between the lidar and the obstacle. The second frame of the third group of lidar point cloud data includes the eighth distance (R 8j ) between the lidar and the obstacle and the eighth angle (θ 8j ) between the lidar and the obstacle. The third frame of the third group of lidar point cloud data includes the ninth distance (R 9j ) between the lidar and the obstacle and the ninth angle (θ 9j ) between the lidar and the obstacle.

2. The method for fitting a straight line based on laser point cloud according to claim 1, wherein The specific initial pose information of the robot includes the x-axis coordinate (x0) of the robot's initial pose, the y-axis coordinate (y0) of the robot's initial pose, and the initial angle (β0) between the lidar and the obstacle; the first set of lidar point cloud data includes the first frame of lidar point cloud data, the second frame of lidar point cloud data, and the third frame of lidar point cloud data in the first set. The first frame of lidar point cloud data in the first set includes the first distance (R 1j ) between the lidar and the obstacle and the first angle (θ 1j ) between the lidar and the obstacle. The second frame of lidar point cloud data in the first set includes the second distance (R 2j ) between the lidar and the obstacle and the second angle (θ 2j ) between the lidar and the obstacle. The third frame of lidar point cloud data in the first set includes the third distance (R 3j ) between the lidar and the obstacle and the third angle (θ 3j ) between the lidar and the obstacle. Among them, the coordinates in the initial pose information of the robot use the robot coordinate system.

3. The method for fitting a straight line based on laser point cloud according to claim 2, wherein The specific robot reference pose information includes the x-axis coordinate (x1) of the robot reference pose, the y-axis coordinate (y1) of the robot reference pose, and the reference angle (β1) between the lidar and the obstacle; the second set of lidar point cloud data includes the first frame of the second set of lidar point cloud data, the second frame of the second set of lidar point cloud data, and the third frame of the second set of lidar point cloud data. The first frame of the second set of lidar point cloud data includes the fourth distance (R 4j ) and the fourth angle (θ 4j ) between the lidar and the obstacle. The second frame of the second set of lidar point cloud data includes the fifth distance (R 5j ) and the fifth angle (θ 5j ) between the lidar and the obstacle. The third frame of the second set of lidar point cloud data includes the sixth distance (R 6j ) and the sixth angle (θ 6j ) between the lidar and the obstacle.

4. The method for fitting a straight line based on laser point cloud according to claim 3, wherein, The specific content of Step S3 includes: Substitute the x-axis coordinate (x0) of the robot initial pose, the y-axis coordinate (y0) of the robot initial pose, and the initial angle (β0) between the lidar and the obstacle in the robot initial pose information obtained in Step S1 into the coordinate transformation formula in the form of parameters; Substitute the first frame, the second frame, and the third frame of the first set of lidar point cloud data in the first set of lidar point cloud data obtained in Step S1 into the coordinate transformation formula in the form of parameters respectively to obtain three sets of coordinate information; Among them, the coordinate transformation refers to the transformation from the robot coordinate system to the world coordinate system; the coordinate information includes the x-axis coordinate in the world coordinate system and the y-axis coordinate in the world coordinate system; the three sets of coordinate information specifically include the first set of coordinate information obtained by substituting the first-frame laser point cloud data of the first group into the coordinate transformation formula, the second set of coordinate information obtained by substituting the second-frame laser point cloud data of the first group into the coordinate transformation formula, and the third set of coordinate information obtained by substituting the third-frame laser point cloud data of the first group into the coordinate transformation formula; the first set of coordinate information includes the first x-axis coordinate (x N1j ) and the first y-axis coordinate (y N1j ), the second set of coordinate information includes the second x-axis coordinate (x N2j ) and the second y-axis coordinate (y N2j ), and the third set of coordinate information includes the third x-axis coordinate (x N3j ) and the third y-axis coordinate (y N3j ).

5. The method for fitting a straight line based on laser point cloud according to claim 4, wherein The coordinate transformation formula for transforming the robot coordinate system into the world coordinate system is: Among them, y Nij refers to the y-axis coordinate of the robot in the world coordinate system, x Nij refers to the x-axis coordinate of the robot in the world coordinate system, R ij refers to the distance between the lidar and the obstacle, θ ij refers to the angle between the lidar and the obstacle; i is the subscript number used to distinguish the group or frame number to which the parameter belongs, i is an integer greater than or equal to 1 and less than or equal to 3, and j is the subscript number used to distinguish the parameters belonging to the same group or the same frame number, j is an integer greater than or equal to 0 and less than or equal to 359.

6. The method for fitting a straight line based on laser point cloud according to claim 5, characterized in that, The coordinate difference formula is: The angle calculation formula is: The obstacle distance calculation formula is: Among them, the fabs function is used to calculate the absolute value; the fmod function is used to calculate the remainder of a floating-point number; the atan2 function is used to calculate the included angle; the sqrt function is used to calculate the square root; the pow function is used for power operation; i is a subscript number used to distinguish the group or frame number to which the parameter belongs, i is an integer greater than or equal to 1 and less than or equal to 3, and j is a subscript number used to distinguish parameters belonging to the same group or the same frame number, j is an integer greater than or equal to 0 and less than or equal to 359.

7. The method for fitting a straight line based on laser point cloud according to claim 6, wherein The specific steps of step S5 include: Perform an average calculation on a total of six frames of data, namely the second group of lidar point cloud data obtained in step S2 and the third group of lidar point cloud data obtained in step S5, to obtain the average lidar point cloud data (T0); Substitute the average lidar point cloud data (T0) into the least squares formula in the form of parameters to obtain the optimal solution of the fitted straight line; Among them, the average lidar point cloud data (T0) includes the average distance (R0) between the lidar and the obstacle.

8. A method for selecting a reference edge based on a laser point cloud, which is implemented on the basis of the method for fitting a straight line based on a laser point cloud according to any one of claims 1 to 7, and is characterized in that, When N fitted straight lines are obtained by the method of fitting a straight line based on lidar point cloud as described in any one of claims 1 to 7 and N is an integer greater than or equal to 2, select the fitted straight line corresponding to the maximum value among the total lengths of the N fitted straight lines as the best parallel wall reference edge; among them, each fitted straight line has a corresponding total length of the fitted straight line.

9. The method for selecting a reference edge based on laser point cloud according to claim 8, wherein The specific steps of selecting the fitted straight line corresponding to the maximum value among the total lengths of the N fitted straight lines as the best parallel wall reference edge include the following steps: Step S601: According to the N fitted straight lines obtained in step S5, select one of the fitted straight lines as the current reference edge, and obtain the length of the current reference edge, then enter step S602; Step S602: Project the remaining N - 1 fitted straight lines in the direction of the current reference edge to obtain N - 1 first projection values corresponding to the remaining N - 1 fitted straight lines, and project the remaining N - 1 fitted straight lines in the direction perpendicular to the current reference edge to obtain N - 1 second projection values corresponding to the remaining N - 1 fitted straight lines, then enter step S603; Step S603: Take the product of the larger value of the first projection value and the second projection value corresponding to the same fitted straight line and the first preset parameter b as the third projection value corresponding to the fitted straight line, and obtain N - 1 third projection values corresponding to the remaining N - 1 fitted straight lines, then enter step S604; Step S604: Calculate the sum of the length of the current reference edge and the N - 1 third projection values corresponding to the remaining N - 1 fitted straight lines, and take the sum of the length of the current reference edge and the N - 1 third projection values corresponding to the remaining N - 1 fitted straight lines as the total length of the fitted straight line corresponding to the current reference edge, then enter step S605; Step S605: Repeat the above steps S601 to S604 until all N fitted straight lines are traversed to obtain N total lengths of the fitted straight lines corresponding to the N fitted straight lines, then enter step S606; Step S606: Select the fitted straight line corresponding to the maximum value among the N total lengths of the fitted straight lines as the best parallel wall reference edge; Among them, in the method of selecting a reference edge based on laser point cloud, the number N of fitted straight lines is an integer greater than or equal to 2.

10. A robot, comprising: One or more processors, one or more chips, and one or more computer programs stored on the chips and executable on the processors, wherein the processors implement the steps of the method according to any one of claims 1 to 9 when executing the computer programs.

11. A chip, characterized in that, The chip stores a computer program, and when the computer program is run by a processor, it implements the steps of the method according to any one of claims 1 to 9.

Images