A fast surface fitting method based on multi-sensor fusion

Abstract

The invention discloses a fast surface fitting method of multi-sensor fusion, belongs to the technical field of building wall construction, is applicable to a wall processing building robot, and comprises the following steps: a depth camera and a laser rangefinder are arranged on a calibration stand, so that the laser lines shot by the two laser rangefinders can be parallel to the optical axis of the depth camera and perpendicular to the wall; a standard plane is set in front of the calibration stand, and the distance between the standard plane and the calibration stand and the horizontal and vertical angles can be adjusted; the data of the laser rangefinder is converted to the depth camera coordinate system, that is, {x1, y1, d}; the information of each point cloud is (c, r) {x, y, z}; the data of the laser rangefinder is replaced and added to the point cloud to form (c, r) {x1, y1, d} data; and a surface fitting is formed based on the above fused data through special random sampling. The fused data does not contain the data generated by the fusion of the laser rangefinder in any filtering and smoothing process, and a single-frame plane can be quickly fitted.

Description

Technical Field

[0001] The invention relates to the technical field of building wall construction, and in particular to a fast surface fitting method of multi-sensor fusion. Background Art

[0002] With the intelligentization of the construction industry, a variety of construction robots have emerged in the construction industry to replace manual labor and achieve safe and efficient construction. Among them, the wall processing robot needs to perceive a lot of data, including the distance and angle between the equipment and the wall currently under construction, whether there are obstacles such as switches in the current wall, and the position and structure of other walls outside the current construction wall. In the detection of the wall, the main body is actually the fitting of the surface. Through the cooperation of multiple sensors, the plane is quickly fitted, and then other logical judgments are performed, including the arrangement plan between sensors, data fusion between sensors, and fast surface fitting calculation based on fused data. Summary of the invention

[0003] The object of the present invention is to provide a fast surface fitting method of multi-sensor fusion to solve the technical problem of fast fitting a plane from sensor data in the prior art.

[0004] The present invention provides a multi-sensor fusion fast surface fitting method, which is applicable to a wall processing construction robot, the robot comprises a chassis, a main lifting structure, a front-end telescopic structure and a main execution structure, the front-end telescopic structure is vertically slidably connected to the surface of the main lifting structure, the main execution structure is horizontally slidably connected to the surface of the front-end telescopic structure, a calibration stand is fixed on the surface of the main execution structure, the calibration stand comprises a depth camera located in the middle, and two laser rangefinders located on both sides, and comprises the following steps:

[0005] Sp1: Mechanical adjustment of horizontal position: The depth camera and laser rangefinder are arranged in the calibration rig through the position of the mechanical structure, so that the laser lines emitted by the two laser rangefinders can be parallel to the optical axis of the depth camera and perpendicular to a standard wall. The positions of the two laser rangefinders are adjusted so that the origin of the two laser rangefinders and the depth camera are in the same straight line;

[0006] Sp2: Post-calibration: A standard plane is set in front of the calibration stand, and the distance between the standard plane and the calibration stand as well as the horizontal and vertical angles can be freely adjusted;

[0007] Sp3: Record data: Convert the data of the laser rangefinder to the depth camera coordinate system, that is, {x1, y1, d}, where x1 and y1 are the position differences between the laser rangefinder and the depth camera in the depth camera coordinate system, and d is the measured straight-line distance from the standard plane to the depth camera;

[0008] Sp4: Get point cloud data: The data acquired by the depth camera is tens of thousands or more point clouds, and the information of each point cloud is (c, r) {x, y, z}, where {x, y, z} is the x, y, z coordinates of the coordinate system of the point to the depth camera, c is the number of rows of the point in the point cloud, r is the number of columns of the point in the point cloud, and multiple sets of data are recorded;

[0009] Sp5: Sensor data fitting: replace the data of the laser rangefinder with the point cloud to form a (c, r){x1, y1, d} data, where (c, r) is set to (-1, 0) or other (c, r) values ​​that would not have existed originally, and the {x, y, z} value is set to {x1, y1, d}, that is, the data of the two sensors are fused;

[0010] Sp6: Surface fitting: The data after the above data fusion must contain special random sampling of the data converted by the laser rangefinder to form a surface fitting.

[0011] Further, the surface fitting process includes the following steps:

[0012] Sp1: Randomly sample K points from the data after data fusion;

[0013] Sp2: Fit a plane model to the K points, that is, fit a plane by the least squares method or other methods;

[0014] Sp3: Calculate the distance from other points to the standard plane. If it is less than a certain threshold, the point is regarded as an inner point, and the number of inner points is counted;

[0015] Sp4: Repeat M times and select the model with the largest number of internal points and a distance to the standard plane less than a certain threshold in the laser rangefinder data;

[0016] Sp5: If within M times, the proportion of internal points is greater than a certain threshold and the distance to the standard plane in the laser rangefinder data is less than a certain threshold, stop repeating and use the current model.

[0017] In the process of sampling based on the fused data, the degree of sampling fitting can be greatly improved, and a suitable plane that satisfies the requirements can be found quickly.

[0018] Furthermore, the data {x1, y1, d} of the laser rangefinder should have one or more points with a distance less than 1 mm in the point cloud at any distance and angle, and this group of points in the point cloud is defined as a matching point set; at any distance and angle, the matching point set obtained is a matching point collection, and the most common (c, r) coordinate point in the matching point collection should have an appearance rate greater than 95%.

[0019] Furthermore, the use of fused data can greatly improve the degree of sampling fitting. When the number of repetitions is M / 3, the conditions for finding a suitable plane can basically be met.

[0020] Furthermore, all the internal points can be used to re-estimate the model.

[0021] Compared with the prior art, the beneficial effects of the present invention are:

[0022] Firstly, in the prior art, in the construction of walls, in the detection of walls, the main body is actually the fitting of surfaces. Through the cooperation of multiple sensors, a plane is quickly fitted, and then other logical judgments are performed, including the arrangement plan between sensors, data fusion between sensors, and rapid surface fitting calculation based on fused data. In view of the above problems, the present invention designs a method for rapid surface fitting of multi-sensor fusion, in which a depth camera and a laser rangefinder are located in the same coordinate system, and the depth camera is used as the origin of the coordinate system. The data of multiple sensors in the coordinate system are fused, and the (c, r) values ​​therein are set to (-1, 0) or other (c, r) values ​​that would not originally exist. The fused data does not include the data generated by the fusion of the laser rangefinder during any filtering and smoothing process of the data, and a single-frame plane can be quickly fitted.

[0023] Secondly, in the present invention, in the process of sampling based on the fused data, the degree of sampling fitting can be greatly improved, and a suitable plane that satisfies the requirements can be found quickly. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0025] Figure 1 A schematic diagram of the installation method of the calibration stand and the wall processing construction robot in the present invention;

[0026] Figure 2 It is a schematic diagram of the process of the present invention.

[0027] Reference numerals:

[0028] 1. Chassis; 2. Main lifting structure; 3. Front end telescopic structure; 4. Main execution structure; 5. Calibration stand. DETAILED DESCRIPTION

[0029] The technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments.

[0030] The components of the embodiments of the present invention generally described and shown in the drawings herein may be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention.

[0031] Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in the field without making any creative work shall fall within the scope of protection of the present invention.

[0032] Combine the following Figure 1 to Figure 2 As shown, an embodiment of the present invention provides a fast surface fitting method of multi-sensor fusion, which is applicable to a wall processing construction robot. The robot includes a chassis 1, a main lifting structure 2, a front telescopic structure 3 and a main execution structure 4. The front telescopic structure 3 is vertically connected to the surface of the main lifting structure 2, and the main execution structure 4 is horizontally connected to the surface of the front telescopic structure 3. A calibration stand 5 is fixed on the surface of the main execution structure 4. The calibration stand 5 includes a depth camera located in the middle and two laser rangefinders located on both sides. The method includes the following steps:

[0033] Sp1: Mechanical adjustment of horizontal position: The depth camera and the laser rangefinder are arranged in the calibration stand 5 through the position of the mechanical structure, so that the laser lines emitted by the two laser rangefinders can be parallel to the optical axis of the depth camera and perpendicular to a standard wall, and the positions of the two laser rangefinders are adjusted so that the origin of the two laser rangefinders and the depth camera are in the same straight line;

[0034] Sp2: Post-calibration: A standard plane is set in front of the calibration stand 5, and the distance between the standard plane and the calibration stand 5 and the horizontal and vertical angles can be freely adjusted;

[0035] Sp3: Record data: Convert the data of the laser rangefinder to the depth camera coordinate system, that is, {x1, y1, d}, where x1 and y1 are the position differences between the laser rangefinder and the depth camera in the depth camera coordinate system, and d is the measured straight-line distance from the standard plane to the depth camera;

[0036] Sp4: Get point cloud data: The data acquired by the depth camera is tens of thousands or more point clouds, and the information of each point cloud is c, r{x, y, z}, where {x, y, z} is the x, y, z coordinates of the coordinate system of the point to the depth camera, c is the number of rows of the point in the point cloud, r is the number of columns of the point in the point cloud, and multiple sets of data are recorded;

[0037] Sp5: Sensor data fitting: replace the data of the laser rangefinder with the point cloud to form a c,r{x1, y1, d} data, where c,r is set to -1, 0 or other c,r values ​​that would not have existed originally, and {x, y, z} values ​​are set to {x1, y1, d}, that is, the data of the two sensors are fused;

[0038] Sp6: Surface fitting: The data after the above data fusion must contain special random sampling of the data converted by the laser rangefinder to form a surface fitting.

[0039] In the prior art, in the construction of walls, in the detection of walls, the main body is actually the fitting of the surface. Through the cooperation of multiple sensors, the plane is quickly fitted, and then other logical judgments are performed, including the arrangement plan between sensors, data fusion between sensors, and fast surface fitting calculation based on fused data. In view of the above problems, the present invention designs a method for fast surface fitting of multi-sensor fusion, in which the depth camera and the laser rangefinder are located in the same coordinate system, and the depth camera is used as the origin of the coordinate system. The data of multiple sensors in the coordinate system are fused, and the c and r values ​​therein are set to -1, 0 or other c and r values ​​that would not have existed originally. The fused data does not include the data generated by the fusion of the laser rangefinder during any filtering and smoothing process of the data, and a single-frame plane can be quickly fitted.

[0040] In another embodiment, the surface fitting process includes the following steps:

[0041] Sp1: Randomly sample K points from the data after data fusion;

[0042] Sp2: Fit a plane model to the K points, that is, fit a plane by the least squares method or other methods;

[0043] Sp3: Calculate the distance from other points to the standard plane. If it is less than a certain threshold, the point is regarded as an inner point, and the number of inner points is counted;

[0044] Sp4: Repeat M times and select the model with the largest number of internal points and a distance to the standard plane less than a certain threshold in the laser rangefinder data;

[0045] Sp5: If within M times, the proportion of internal points is greater than a certain threshold and the distance to the standard plane in the laser rangefinder data is less than a certain threshold, stop repeating and use the current model.

[0046] In the process of sampling based on the fused data, the degree of sampling fitting can be greatly improved, and a suitable plane can be found quickly. When the number of repetitions is M / 3, the conditions for finding a suitable plane can basically be met, and all the internal points can be used to re-estimate the model.

[0047] In another embodiment, the data {x1, y1, d} of the laser rangefinder should have one or more points with a distance less than 1 mm in the point cloud at any distance and angle, and this group of points in the point cloud is defined as a matching point set; at any distance and angle, the obtained matching point set is a matching point collection, and the appearance rate of the same c, r coordinate point that appears most frequently in the matching point collection should be greater than 95%.

[0048] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A multi-sensor fusion fast surface fitting method for a wall surface processing construction robot, the robot comprising a chassis (1), a main lifting structure (2), a front telescopic structure (3) and a main execution structure (4), the front telescopic structure (3) being vertically connected to the surface of the main lifting structure (2), and the main execution structure (4) being horizontally connected to the surface of the front telescopic structure (3), characterized in that: A calibration stand (5) is fixed on the surface of the main execution structure (4), and the calibration stand (5) includes a depth camera located in the middle and two laser rangefinders located on both sides, including the following steps: Sp1: Mechanical adjustment of horizontal position: The depth camera and the laser rangefinder are arranged in the calibration stand (5) through the position of the mechanical structure, so that the laser lines emitted by the two laser rangefinders can be parallel to the optical axis of the depth camera and perpendicular to a standard wall, and the positions of the two laser rangefinders are adjusted so that the origin of the two laser rangefinders and the depth camera are in the same straight line; Sp2: Post-calibration: A standard plane is set in front of the calibration stand (5), and the distance between the standard plane and the calibration stand (5) and the horizontal and vertical angles can be freely adjusted; Sp3: Record data: Convert the data of the laser rangefinder to the depth camera coordinate system, that is, {x1, y1, d}, where x1 and y1 are the position differences between the laser rangefinder and the depth camera in the depth camera coordinate system, and d is the measured straight-line distance from the standard plane to the depth camera; Sp4: Get point cloud data: The data acquired by the depth camera is tens of thousands or more point clouds, and the information of each point cloud is (c, r) {x, y, z}, where {x, y, z} is the x, y, z coordinates of the coordinate system of the point to the depth camera, c is the number of rows of the point in the point cloud, r is the number of columns of the point in the point cloud, and multiple sets of data are recorded; Sp5: Sensor data fitting: replace the data of the laser rangefinder with the point cloud to form a (c, r){x1, y1, d} data, where (c, r) is set to (-1, 0) or other (c, r) values ​​that would not have existed originally, and the {x, y, z} value is set to {x1, y1, d}, that is, the data of the two sensors are fused; Sp6: Surface fitting: The data after the above data fusion must contain special random sampling of the data converted by the laser rangefinder to form a surface fitting.

2. The fast surface fitting method of multi-sensor fusion according to claim 1, characterized in that: The surface fitting process comprises the following steps: Sp1: Randomly sample K points from the data after data fusion; Sp2: Fit a plane model to the K points, that is, fit a plane by the least squares method or other methods; Sp3: Calculate the distance from other points to the standard plane. If it is less than a certain threshold, the point is regarded as an inner point, and the number of inner points is counted; Sp4: Repeat M times and select the model with the largest number of internal points and a distance to the standard plane less than a certain threshold in the laser rangefinder data; Sp5: If within M times, the proportion of internal points is greater than a certain threshold and the distance to the standard plane in the laser rangefinder data is less than a certain threshold, stop repeating and use the current model.

3. The fast surface fitting method of multi-sensor fusion according to claim 1, characterized in that: The data {x1, y1, d} of the laser rangefinder should have one or more points with a distance less than 1 mm in the point cloud at any distance and angle, and this group of points in the point cloud is defined as a matching point set; at any distance and angle, the matching point set obtained is a matching point collection, and the occurrence rate of the most common (c, r) coordinate point in the matching point collection should be greater than 95%.

4. The fast surface fitting method of multi-sensor fusion according to claim 2, characterized in that: The model can also be re-estimated using all interior points.

Images