A multi-robot collaborative three-dimensional measurement operation method

Abstract

The present invention discloses a multi-robot collaborative three-dimensional measurement operation method. First, a virtual simulation environment is built on a computer, and the measurement poses and measurement paths of the two robots are set on the virtual simulation environment, and the measurement paths are evaluated for path evaluation. Then, hand-eye calibration is completed in the real measurement environment, and the measurement paths that pass the path evaluation are sent to the corresponding physical robots 1 and 2 to obtain point cloud images from multiple perspectives. The corresponding point cloud of the object to be measured is obtained through coordinate transformation. Finally, the point cloud of the physical robot 2 is transformed into the base coordinate system of the physical robot 1 to obtain the point cloud I of the object to be measured obtained by the two robots. B Through the virtual simulation platform of the present invention, the path planning of the two robots effectively reduces the trial-and-error risk of multi-perspective measurement, reduces the burden of subsequent registration tasks, improves the working efficiency of the structured light measuring instrument, shortens the task cycle. At the same time, the use of two robots enables a larger measurement range and can complete the measurement tasks of larger objects compared to a single robot.

Description

[0001] Technical Field

[0002] The present invention belongs to the technical field of three-dimensional shape measurement, and more specifically, relates to a multi-robot collaborative three-dimensional measurement operation method. Background Art

[0003] In recent years, with the rapid development of machine vision, structured light measurement technology has been widely applied to some high-precision measurement fields such as impact detection, cultural relic reconstruction, and medical scanning due to advantages such as high-precision and good-quality point clouds generated. Due to the limited imaging range and shooting angle of the structured light measurement device, it is impossible to obtain the complete three-dimensional shape information of the object to be measured at one time when performing an actual three-dimensional shape measurement task. Generally, the solution is to move the structured light measurement device to the spatial poses corresponding to each perspective, obtain the point cloud information of the object to be measured from multiple perspectives, and then splice the multi-perspective point cloud information to make the point cloud information of the object to be measured complete. However, due to the large mass of high-precision structured light devices, the traditional manual moving method cannot accurately grasp the pose states of each acquisition perspective. In some environments with high load and high repeatability, robots can well replace traditional human labor, and pre-path planning can be performed on the robots to complete the measurement task.

[0004] In factories, the measurement path generation of traditional robots usually adopts the method of manual teaching, and the operator selects the path points of the robot measurement task one by one for the robot, which requires a high level of technical skills of the operator and familiarity with the on-site working environment. Since the accuracy of the single-perspective point cloud is greatly affected by the ranging of the structured light device, when facing a measurement device such as a structured light measuring instrument that has requirements for the shooting distance, the operator also needs to manually measure the distance between the measurement device and the object to be measured and judge whether it meets the shooting distance requirements. However, in some measurement occasions where it is difficult to obtain the distance between the measurement device and the object to be measured, such as in high-temperature environments, how to ensure that the distance between the measurement device and the object to be measured meets the requirements at each measurement pose has become a difficult problem. Therefore, it is necessary to adopt an effective measurement method to reduce the operation difficulty of the robot in the measurement task and accurately obtain the distance between the measurement device and the object to be measured, so as to obtain a higher-quality single-perspective point cloud. Summary of the Invention

[0005] The purpose of the present invention is to overcome the deficiencies of the prior art and provide a multi-robot collaborative three-dimensional measurement operation method to improve the working efficiency of the structured light measuring instrument, shorten the task cycle, provide a good initial value for the subsequent fine splicing process, and reduce the burden of the subsequent registration task.

[0006] To achieve the above object of the invention, the multi-robot collaborative three-dimensional measurement operation method of the present invention is characterized by including:

[0007] (1) Virtual simulation environment construction

[0008] 1.1) According to the actual measurement environment, build a virtual simulation environment on a computer equipped with an open-source robot system, and import the description files of the dual robots into the virtual simulation environment to generate corresponding Robot 1 and Robot 2. At the same time, in the virtual simulation environment, install Structured Light Measuring Instrument 1 and Depth Camera 1 on the fixed fixture of the end flange of Robot 1, and install Structured Light Measuring Instrument 2 and Depth Camera 2 on the fixed fixture of the end flange of Robot 2;

[0009] 1.2) Simulate the object to be measured and place it between Robot 1 and Robot 2. Both Structured Light Measuring Instrument 1 and Structured Light Measuring Instrument 2 are facing the object to be measured directly. Determine multiple measurement planes for Structured Light Measuring Instrument 1 and Structured Light Measuring Instrument 2 to photograph the object to be measured, and multiple measurement paths existing on each measurement plane. For Structured Light Measuring Instrument 1: The i-th measurement path of the k-th measurement plane is denoted as where, K R1 is the number of measurement planes, M R1_k is the number of measurement paths on the k-th measurement plane. At the same time, each measurement path also contains multiple measurement points. The j-th measurement point in the i-th path of the k-th measurement plane of the object to be measured, that is, the measurement pose, is denoted as is the number of measurement poses on the measurement path of the k-th measurement plane. R1 represents Robot 1. For Structured Light Measuring Instrument 2: The i-th measurement path of the k-th measurement plane is denoted as where, K R2 is the number of measurement planes, M R2_k is the number of measurement paths on the k-th measurement plane. The j-th measurement point in the i-th path of the k-th measurement plane of the object to be measured, that is, the measurement pose, is denoted as is the number of measurement poses on the measurement path of the k-th measurement plane. R2 represents Robot 2;

[0010] The field of view ranges of both Structured Light Measuring Instrument 1 and Structured Light Measuring Instrument 2 are rectangles with a length of m centimeters and a width of n centimeters. For Structured Light Measuring Instrument 1, the length of the circumscribed rectangle of the k-th measurement plane of the object to be measured is The width is The measurement paths are longitudinal paths along the length direction, one by one. Then the constraint needs to be satisfied: the number of measurement paths on the same measurement plane The number of measurement points on the same measurement path According to this constraint, the j-th measurement pose of the i-th measurement path of the k-th measurement plane is obtained For Structured Light Measuring Instrument 2, the length of the circumscribed rectangle of the k-th measurement plane of the object to be measured is The width is If the measurement paths are longitudinal paths along the length direction, one by one, the following constraints need to be satisfied: the number of measurement paths on the same measurement surface the number of measurement points on the same measurement path According to this constraint, the j-th measurement pose of the i-th measurement path on the k-th measurement surface is obtained

[0011] (2) Measurement path planning

[0012] 2.1) For the i-th measurement path on the k-th measurement surface of robot 1 First, in the virtual simulation environment, drag the end of the robot to the measurement pose where the measurement pose is is the position coordinate of the end of robot 1, is the attitude coordinate of the end of robot 1;

[0013] 2.2) At the measurement pose of robot 1 acquire the image of the object to be measured by using a depth camera, obtain the RGB-D image and convert it to a grayscale image, then perform threshold segmentation to filter the background, and extract the contour information of the object to be measured to obtain the minimum circumscribed rectangle where the contour of the object to be measured is located After that, traverse the depth information d of all pixel points in the range of the minimum circumscribed rectangle corresponding to the RGB-D image, find the shortest distance d min between the object to be measured and the structured light measuring instrument, and record the pixel point coordinates a(u, v) of the shortest distance at the same time. Then judge whether the shortest distance d min meets the measurement range of the structured light measuring instrument: d min ∈[D - δ, D + δ], where D is the focal length during camera calibration of the structured light measuring instrument, and δ is the allowable error range of measurement of the structured light measuring instrument; if not satisfied, go to step 2.3) to adjust the measurement pose, if satisfied, go to step 2.4);

[0014] 2.3) Convert the pixel point coordinates a(u, v) through coordinate system transformation to obtain the corresponding spatial coordinates (x (u,v) , y (u,v) , z (u,v) ), and then combine with the position coordinate of the end of robot 1 to determine a spatial straight line:

[0015]

[0016] where x, y, z are the coordinates on the spatial straight line;

[0017] Then, from the position coordinate Starting from this point, along this space straight line, a position coordinate is calculated. Meet the measurement conditions:

[0018]

[0019] Obtain the position coordinates that meet the measurement conditions After that, combine with the original pose coordinates Form a measurement pose As the measurement pose According to the measurement pose The corresponding joint states of the robot are inversely solved by robot kinematics to achieve pose adjustment;

[0020] 2.4), Record the adjusted measurement pose and return to step 2.1) to perform the next measurement pose Set until the measurement path k S i All measurement poses k P ij The shortest distance d min After the judgment is completed, then enter step (3). Among them, before setting the next measurement pose If:

[0021] A. When the measurement pose is set at a spatial pose where the degrees of freedom of the robot degenerate and the inverse kinematics has no solution, that is, near the singular point, in order to ensure the smooth movement of the robot, a transition point needs to be set to avoid the singular point of the robot;

[0022] B. When the shape of the measured object is irregular and there are protrusions on the surface of the measured object, in order to obtain better measurement results and avoid collisions with the irregular measured object during the movement of the robot carrying the structured light measuring instrument, when setting the measurement pose the actual measurement pose of the robot and the morphological characteristics of the measured object need to be considered. A transition point is set between the measurement pose and the measurement pose

[0023] The transition point does not require distance detection and adjustment. The transition point is for artificially adjusting the measurement path of the robot;

[0024] 2.5), Evaluate the measurement path for

[0025] All measurement poses and all transition points Form a complete measurement path For the measurement path Perform path evaluation, and the steps are as follows:

[0026] Step 2.5.1): Execute the measurement path in the virtual simulation environment Robot 1 will move continuously from the measurement pose to the measurement pose Record the process points by equidistant sampling during the movement process t is the moment, T is the moment of the movement end point, and the set constitutes the actual movement path of the robot as

[0027] In the measurement path its starting position coordinates are the ending position coordinates are Then the shortest distance between the ending position and the starting position is:

[0028]

[0029] In the actual movement path its starting position coordinates are the ending position coordinates are Then the length of the actual movement of the robot is:

[0030]

[0031] Subtracting the two equations gives:

[0032] L = l2 - l1

[0033] Then the evaluation function f1(L) is expressed as: f1(L) = (δ1 - L) / δ1, 0 ≤ L ≤ δ1, where δ1 is the set maximum error threshold;

[0034] Step 2.5.2): Find a pose in the actual movement path of robot 1 to the centroid coordinates (Px obj , Py obj , Pz obj ) of the measured substance, and the distance l3 is the smallest. Then the distance l3 is expressed as:

[0035]

[0036] Among them, is the position coordinate of the pose ;

[0037] Then the evaluation function f2(l3) is expressed as:

[0038] f2(l3) = (l3 - δ2) / l3

[0039] where δ2 is the minimum distance to ensure that the structured light measuring instrument does not collide with the object to be measured;

[0040] 2.5.3) For the measurement path The comprehensive evaluation function k F i is:

[0041]

[0042] According to the comprehensive evaluation function k F i evaluate the set measurement path. When the comprehensive evaluation function is satisfied, it passes the evaluation and enters step 2.7). Otherwise, go to step 2.6), where g is a threshold defined by the measurement personnel according to different measurement scenarios, and 0 < g < 100;

[0043] 2.6) Traverse all poses in the measurement path to find the pose with the closest distance to the pose Then move the robot to the measurement pose and manually drag the end of the robot to increase the distance d min between the current measurement pose of the robot and the object to be measured. If the pose is a measurement pose, the adjustment range still needs to meet the measurement conditions of the structured light measuring instrument: d min ∈[D - δ, D + δ]. If the pose is a transition point, adjust the distance d min according to the setting conditions of the transition point. Replace the pose with the adjusted pose to complete the correction of the measurement path and then enter step 2.7);

[0044] 2.7) Process all measurement paths of all measurement surfaces according to steps 2.1) to 2.6) to complete the measurement path planning simulation. Convert the planned measurement path into a communication message that the robot can recognize and send it to the physical robot 1 by the virtual simulation environment;

[0045] For robot 2, obtain the planned measurement path by the method of steps 2.1) - 2.7) and convert it into a communication message that the robot can recognize and send it to the physical robot 2 by the virtual simulation environment;

[0046] (3) Real - environment measurement

[0047] 3.1). For the physical robot 1, perform hand-eye calibration to obtain the rotation and translation matrix of the camera coordinate system relative to the end flange coordinate system of the physical robot 1, that is, the hand-eye relationship matrix R1 H cg ;

[0048] 3.2). After completing the hand-eye calibration, start measuring the object to be measured. After the physical robot 1 receives the measurement path sent by the virtual simulation environment, it moves to the measurement pose in sequence;

[0049] 3.3.). When reaching the measurement pose , the physical robot 1 sends the measurement pose to the structured light measuring instrument, and obtains the single-viewpoint cloud of the object to be measured at the current measurement pose through the binocular vision camera and uses the hand-eye relationship matrix R1 H cg , the pose matrix from the end flange coordinate system of the physical robot 1 to the base coordinate system of the physical robot 1 obtained according to the measurement pose to transform the single-viewpoint cloud to the base coordinate system of the physical robot 1;

[0050] In this way, the point cloud of the object to be measured in the base coordinate system of the physical robot 1 R1 I B is obtained, that is, the rough stitching result of the point cloud of the object to be measured;

[0051] 3.4.). For the physical robot 2, according to the method in steps 3.1)-3.3), obtain the point cloud of the object to be measured in the base coordinate system of the physical robot 1 R2 I B that is, the rough stitching result of the point cloud of the object to be measured;

[0052] 3.5). Obtain the rotation and translation matrix for the base coordinate system of the physical robot 2 to reach the base coordinate system of the physical robot 1 through rotation and translation

[0053] 3.5.1). First, fix a probe 1 and a probe 2 on the end flanges of the physical robot 1 and the physical robot 2 respectively, and set the TCP tool coordinate systems of the probe 1 and the probe 2 at the robot end;

[0054] 3.5.2) After setting the TCP tool coordinate systems of probe 1 and probe 2, manually operate the physical robot 1 and the physical robot 2 to simultaneously point probe 1 and probe 2 at the same point within the working spaces of the two physical robots. Record the pose P1 of the end of probe 1 in the coordinate system of physical robot 1 and the pose P2 of the end of probe 2 in the coordinate system of robot 2. Obtain the rotation and translation matrix H between the TCP coordinate system of the probe end and the base coordinate systems of physical robots 1 and 2 based on the poses P1 and P2 g1 and H g2 Then can be expressed as:

[0055]

[0056] 3.6) Obtain the point cloud of the object to be measured R2 I B Convert it to the coordinate system of physical robot 1 to obtain the point cloud of the object to be measured R2 I′ B :

[0057]

[0058] In this way, the point cloud I of the object to be measured obtained by the dual robots is obtained B :

[0059] I B = R1 I B + R2 I′ B .

[0060] The invention object of the present invention is realized as follows:

[0061] The multi-robot collaborative three-dimensional measurement operation method of the present invention first constructs a virtual simulation environment on a computer equipped with an open-source robot system (ROS) according to the actual measurement environment; then, on the premise of ensuring the integrity of three-dimensional topography measurement, the measurement poses and measurement paths of the two robots are set on the virtual simulation environment and the measurement paths are evaluated; then, in the actual measurement environment, the camera calibration of the structured light measuring instrument and the hand-eye calibration between the structured light measuring instrument and the two robots are completed, and the measurement paths passed through the path evaluation are sent to the corresponding physical robots 1 and 2. The physical robots 1 and 2 carry the structured light measuring instrument and successively reach the set measurement poses to take pictures, obtaining point cloud images from multiple perspectives; then, according to the hand-eye calibration results, the point clouds from multiple perspectives are unified into the base coordinate systems of the physical robots 1 and 2 through coordinate transformation to obtain the corresponding point cloud of the object to be measured. Finally, according to the rotation and translation matrix of the base coordinate system of the physical robot 2 reaching the base coordinate system of the physical robot 1 through rotation and translation, the point cloud of the physical robot 2 is transformed into the base coordinate system of the physical robot 1 to obtain the point cloud I of the object to be measured obtained by the two robots. B . Through the virtual simulation platform, the robot path planning of the present invention effectively reduces the trial-and-error risk of multi-perspective measurement; by transforming the point cloud into the robot base coordinate system to complete the rough registration of the point cloud, it provides good prior conditions for the subsequent fine registration process, reduces the burden of the subsequent registration task, improves the working efficiency of the structured light measuring instrument, shortens the task cycle. At the same time, using two robots enables a larger measurement range and can complete the measurement tasks of larger objects compared to a single robot.

[0062] Specifically, the present invention has the following advantages and innovations:

[0063] (1) The present invention constructs a virtual simulation scenario on ROS and can conduct full simulation experiments in the simulation environment, reducing the trial-and-error risk when selecting measurement poses;

[0064] (2) By detecting the distance to judge whether the distance from the structured light measuring instrument to the object to be measured at the current pose meets the ranging requirements of the structured light measuring instrument, the measurement accuracy and shooting effect are effectively improved, and the automatic adjustment of the measurement pose is realized for the measurement poses with inappropriate ranging;

[0065] (3) According to a specific path evaluation function, the selection criteria for a more efficient and safer path are given, improving the overall measurement efficiency of the system and ensuring measurement safety;

[0066] (4) The point clouds of the object to be measured taken at different measurement poses are unified into the robot base coordinate system, realizing the rough registration of the point cloud and providing a good initial value for the subsequent fine registration process.

[0067] (5) The entire measurement process uses a binocular structured light measuring instrument and a non-contact measurement method to measure the object to be measured. There is no need to attach marking points on the surface of the object to be measured. It retains more morphological information of the object to be measured and also has the advantages of high accuracy and high speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 This is a flowchart of a specific implementation of the multi-robot collaborative three-dimensional measurement operation method of the present invention;

[0069] Figure 2 It is a schematic diagram for determining the measurement path and measurement posture;

[0070] Figure 3 It is a schematic diagram of the transformation of the rough splicing coordinate system;

[0071] Figure 4 It is a schematic diagram of a specific instance of the constructed virtual simulation environment;

[0072] Figure 5 It is a schematic diagram of the measurement posture setting on the same measurement surface of the object being measured and a single-view point cloud diagram;

[0073] Figure 6 It is a schematic diagram of the settings of different measurement surfaces of the object being measured and the rough splicing results of a single measurement surface;

[0074] Figure 7 It is the point cloud result of the measured object image in the base coordinate system of physical robot 1 and physical robot 2, where (a) is the point cloud in the base coordinate system of physical robot 1, and (b) is the point cloud in the base coordinate system of physical robot 2;

[0075] Figure 8 It is the rough stitching result of the final point cloud of the measured object. DETAILED DESCRIPTION

[0076] The following describes the specific embodiments of the present invention in conjunction with the accompanying drawings so that those skilled in the art can better understand the present invention. It should be noted that in the following description, when detailed descriptions of known functions and designs may dilute the main content of the present invention, such descriptions will be omitted here.

[0077] Figure 1 It is a flow chart of a specific implementation of the multi-robot collaborative three-dimensional measurement operation method of the present invention.

[0078] In this embodiment, if Figure 1 As shown, the three-dimensional point cloud measurement method based on robot path planning of the present invention includes the following steps:

[0079] Step S1: Virtual simulation environment construction

[0080] Step S1.1: Generate robots 1 and 2 and install structured light measuring instruments and depth cameras

[0081] According to the actual measurement environment, build a virtual simulation environment on a computer equipped with an open-source robot system, and import the description files of the dual robots into the virtual simulation environment to generate the corresponding robot 1 and robot 2. At the same time, in the virtual simulation environment, install the structured light measuring instrument 1 and the depth camera 1 on the fixed fixture of the end flange of robot 1, and install the structured light measuring instrument 2 and the depth camera 2 on the fixed fixture of the end flange of robot 2.

[0082] Among them, the description file is a URDF (Universal Robot Description Format) description file, and the contents include: links, joints, kinematic and dynamic parameters, visualization models, collision detection models, etc.

[0083] Step S1.2: Determine the measurement path

[0084] Simulate the object to be measured and place it between robots 1 and 2. The structured light measuring instrument 1 and the structured light measuring instrument 2 are both facing the object to be measured, and determine multiple measurement surfaces for the structured light measuring instruments 1 and 2 to photograph the object to be measured and multiple measurement paths existing on each measurement surface.

[0085] In order to obtain the three-dimensional shape characteristics of the object to be measured more completely and accurately, and ensure that there is an overlapping area between adjacent point clouds, simulate the object to be measured and place it directly in front of the robot and the structured light measuring instrument, and determine multiple measurement surfaces for the structured light measuring instrument to photograph the object to be measured and multiple measurement paths existing on each measurement surface.

[0086] Figure 2 It is a schematic diagram of robot 1 determining the measurement path and measurement pose. In this embodiment, as Figure 2 shown, for the structured light measuring instrument 1: The i-th measurement path of the k-th measurement surface is denoted as where, K R1 is the number of measurement surfaces, M R1_k is the number of measurement paths on the k-th measurement surface. At the same time, each measurement path also contains multiple measurement points. The j-th measurement point, that is, the measurement pose, on the i-th path of the k-th measurement surface of the object to be measured is denoted as is the number of measurement poses on the measurement path of the k-th measurement surface, and R1 represents robot 1.

[0087] Correspondingly, for the structured light measuring instrument 2: The i-th measurement path of the k-th measurement surface is denoted as where, K R2 is the number of measurement surfaces, M R2_kis the number of measurement paths on the k-th measurement plane, and the j-th measurement point on the i-th path on the k-th measurement plane of the object to be measured, that is, the measurement pose, is denoted as is the number of measurement poses on the measurement path of the k-th measurement plane, and R2 represents Robot 2.

[0088] The field of view ranges of structured light measuring instruments 1 and 2 are both rectangles with a length of m centimeters and a width of n centimeters. For structured light measuring instrument 1, the length of the circumscribed rectangle of the k-th measurement plane of the object to be measured is and the width is If the measurement paths are longitudinal paths one by one along the length direction, the following constraints need to be satisfied: the number of measurement paths on the same measurement plane the number of measurement points on the same measurement path According to this constraint, the j-th measurement pose of the i-th measurement path on the k-th measurement plane is obtained such as Figure 2 shown, there are overlapping regions both horizontally and vertically.

[0089] For structured light measuring instrument 2, the length of the circumscribed rectangle of the k-th measurement plane of the object to be measured is and the width is If the measurement paths are longitudinal paths one by one along the length direction, the following constraints need to be satisfied: the number of measurement paths on the same measurement plane the number of measurement points on the same measurement path According to this constraint, the j-th measurement pose of the i-th measurement path on the k-th measurement plane is obtained

[0090] Step S2: Measurement path planning

[0091] Step S2.1: For the i-th measurement path on the k-th measurement plane of Robot 1 First, in the virtual simulation environment, drag the end of the robot to the measurement pose where the measurement pose is is the position coordinate of the end of Robot 1, and

[0092] is the attitude coordinate of the end of Robot 1.

[0093] At the measurement pose of Robot 1 by using a depth camera to collect the image of the object to be measured, obtaining the RGB-D image and converting it into a grayscale image, then performing threshold segmentation to filter the background, and then extracting the contour information of the object to be measured to obtain the minimum circumscribed rectangle where the contour of the object to be measured is located After that, traverse the depth information d of all pixel points within the minimum bounding rectangle corresponding to the RGB-D image to find the shortest distance d between the object under test and the structured light measuring instrument. min , and record the pixel coordinates a(u, v) of the shortest distance at the same time. Then, judge whether the shortest distance d min meets the measurement range of the structured light measuring instrument: d min ∈ [D - δ, D + δ], where D is the focal length during camera calibration of the structured light measuring instrument, and δ is the allowable error range for the measurement of the structured light measuring instrument; if not satisfied, go to step S2.3 to adjust the measurement pose, and if satisfied, go to step S2.4.

[0094] Step S2.3: Pose adjustment

[0095] Convert the pixel coordinates a(u, v) through the coordinate system transformation to obtain the corresponding spatial coordinates (x (u,v) , y (u,v) , z (u,v) ), and then combine with the position coordinates at the end of the robot 1 to determine a spatial straight line:

[0096]

[0097] where x, y, and z are the coordinates on the spatial straight line;

[0098] Then, starting from the position coordinates , along this spatial straight line, calculate a position coordinate that meets the measurement conditions:

[0099]

[0100] Obtain the position coordinate that meets the measurement conditions After that, combine with the original pose coordinates to form a measurement pose as the measurement pose According to the measurement pose Use the inverse kinematics of the robot to solve the corresponding joint states of the robot and realize pose adjustment.

[0101] Step S2.4: Repeat steps S2.1 - S2.3 until the shortest distance judgment and pose adjustment of all measurement poses on the measurement path are completed

[0102] Record the measurement pose , return to step S2.1, and perform the next measurement pose setting until the measurement path k S i all measurement poses k Pij The shortest distance d min The judgment is completed, and then, proceed to step S2.5, where, when performing the next measurement pose Before setting, if:

[0103] A. The measurement pose is set at a spatial pose where the degrees of freedom of the robot degenerate and the inverse kinematics has no solution, that is, near a singular point. In order to ensure the smooth movement of the robot, a transition point needs to be set to avoid the singular point of the robot;

[0104] B. When the shape of the measured object is irregular and there are protrusions on the surface of the measured object, in order to obtain better measurement results and avoid collisions with the irregular measured object during the movement of the robot carrying the structured light measuring instrument, when setting the measurement pose it is necessary to consider the actual measurement pose of the robot and the morphological characteristics of the measured object

[0105] to adjust the measurement path of the measured object.

[0106] Step S2.5: Evaluate the measurement path k S i for path evaluation

[0107] All measurement poses and all transition points constitute a complete measurement path In order to measure the measurement path to determine whether it can be used as an executable path for the robot to complete the measurement task, evaluate the measurement path from the perspectives of efficiency and safety as follows:

[0108] Step S2.5.1: Execute the measurement path in the virtual simulation environment Robot 1 will move continuously from the measurement pose to the measurement pose Record the process points by equal-time sampling during the movement process t is the time, T is the time at the end of the movement, and the set constitutes the actual movement path of the robot as

[0109] In the measurement path its starting position coordinates are and the ending position coordinates are Then the shortest distance between the ending position and the starting position is:

[0110]

[0111] In the actual motion path the starting position coordinates are and the ending position coordinates are Then the length of the actual motion of the robot is:

[0112]

[0113] Subtracting the two equations gives:

[0114] L = l2 - l1

[0115] Then the evaluation function f1(L) is expressed as: f1(L) = (δ1 - L) / δ1, 0 ≤ L ≤ δ1, where δ1 is the set maximum error threshold.

[0116] Step S2.5.2: Find a pose in the actual motion path of robot 1 to the centroid coordinates (Px obj , Py obj , Pz obj ) of the measured object such that the distance l3 is minimized. Then the distance l3 is expressed as:

[0117]

[0118] where is the position coordinates of the pose ;

[0119] Then the evaluation function f2(l3) is expressed as:

[0120] f2(l3) = (l3 - δ2) / l3

[0121] where δ2 is the minimum distance to ensure that the structured light measuring instrument does not collide with the measured object.

[0122] Step S2.5.3: The comprehensive evaluation function for the measurement path k F i is:

[0123]

[0124] According to the comprehensive evaluation function k F i evaluate the set measurement path. When the comprehensive evaluation function the path passes the evaluation and proceeds to step S2.7; otherwise, proceed to step S2.6, where g is a threshold defined by the measurement personnel according to different measurement scenarios, 0 < g < 100.

[0125] Step S2.6: Correct the path

[0126] Traverse the measurement path for all poses, and find the pose closest to the pose After that, move the robot to the measurement pose and manually drag the end of the robot to increase the distance d between the current measurement pose of the robot and the object to be measured min . If the pose is the measurement pose, the adjustment range still needs to meet the measurement conditions of the structured light measuring instrument: d min ∈[D - δ, D + δ]. If the pose is a transition point, the distance d needs to be adjusted according to the setting conditions of the transition point min . Replace the pose with the adjusted pose, thus completing the correction of the measurement path , and then enter step S2.7

[0127] Step S2.7: Process all measurement paths of all measurement surfaces according to steps S2.1 to S2.6 to complete the simulation of measurement path planning, and convert the planned measurement path into a communication message that the robot can recognize and send it to the physical robot by the virtual simulation environment

[0128] For robot 2, obtain the planned measurement path according to the method of steps S2.1 to S2.7 and convert it into a communication message that the robot can recognize and send it to the physical robot 2 by the virtual simulation environment

[0129] Step S3: Real - environment measurement

[0130] Step S3.1: Perform hand - eye calibration on the physical robot 1 to obtain the rotation - translation matrix, i.e., the hand - eye relationship matrix R1 H cg between the camera coordinate system and the end - flange coordinate system of the physical robot 1

[0131] Figure 3 is the schematic diagram of the rough - stitching coordinate system transformation. As Figure 3 shown, the positional relationship among the physical robot, the object to be measured, and the structured - light scanner (camera) and the transformation between coordinate systems are as Figure 3 shown. Among them, the camera coordinate system is represented by the subscript c, the end - flange coordinate system of the robot is represented by the subscript g, and the base coordinate system of the robot is represented by the subscript B. Before the robot carries the scanner to complete the measurement task, it is necessary to perform hand - eye calibration to solve the relative - position relationship matrix R1 H cg between the structured - light measuring instrument and the end - flange of the physical robot. Specifically (for simplicity, here the robot is robot 1 or 2, and the solution method is the same):

[0132] Step S3.1.1: First, adjust the binocular structured light camera of the structured light scanner so that both the left and right cameras can capture the object to be measured, and complete camera calibration according to the Zhang Zhengyou calibration method.

[0133] Step S3.1.2: Manually operate the entity robot to capture the calibration board from the q = 1, 2,...... r poses respectively, obtain the calibration board pictures corresponding to the poses, and record the pose coordinates of the robot end flange at the same time.

[0134] Step S3.1.3: Obtain the rotation vector R of the calibration board relative to the camera in each group of calibration board pictures according to the Zhang's calibration method cq and the translation vector T cq , and form a rotation and translation matrix from them, that is, obtain the external parameter matrix of the camera The pose matrix of the robot can be calculated according to the pose coordinates of the robot end flange

[0135] Step S3.1.4: Since the relationship between the robot base and the calibration board during any two (the p-th and q-th) pose transformations then there is:

[0136]

[0137]

[0138] where, is the rotation and translation matrix from the robot end flange to the robot base coordinate system during the p-th shooting, is the hand-eye relationship matrix during the p-th shooting, is the rotation and translation matrix from the camera to the robot coordinate system during the p-th shooting. has the same meaning as that during the p-th shooting.

[0139] Since the relationship between the robot base and the calibration board remains unchanged during the two shootings, there is a rotation and translation matrix H from the robot base to the calibration board rc :

[0140]

[0141] Since the pose relationship between the robot flange end and the camera remains unchanged during the two transformations, there is a hand-eye relationship matrix H cg :

[0142]

[0143] Then the processed hand-eye calibration equation can be written as:

[0144] AX = XB

[0145] In the formula

[0146] Step S3.1.5: Use the Tsai-Lenz algorithm to solve the hand-eye calibration equation, and then obtain the rotation and translation matrix of the camera coordinate system relative to the robot end flange coordinate system, that is, the hand-eye relationship matrix H cg . For robot 1, the hand-eye relationship matrix H cg is R1 H cg . For robot 1, the hand-eye relationship matrix H cg is R2 H cg .

[0147] After completing the hand-eye calibration, start measuring the object to be measured. After the physical robot 1 receives the measurement path sent by the virtual simulation environment, it moves to the measurement pose in sequence;

[0148] Step S3.3: When reaching the measurement pose , the physical robot 1 sends the measurement pose to the structured light measuring instrument, and obtains the single-viewpoint cloud of the object to be measured at the current measurement pose through the binocular vision camera and uses the hand-eye relationship matrix R1 H cg , and the pose matrix from the end flange coordinate system of the physical robot 1 to the base coordinate system of the physical robot 1 obtained according to the measurement pose to convert the single-viewpoint cloud to the base coordinate system of the physical robot 1. Specifically:

[0149] Step S3.3.1: At the measurement pose , the left and right cameras of the structured light measuring instrument each take a depth image, and perform point cloud reconstruction according to the binocular camera principle to obtain the single-viewpoint cloud at the measurement pose and obtain the pose matrix according to the measurement pose

[0150] Step S3.3.2: Convert the single-viewpoint cloud to the robot base coordinate system to form a new point cloud . The conversion method is as follows:

[0151]

[0152] Step S3.4: Repeat steps S3.2 and S3.3 to obtain the point cloud of the object to be measured in the robot base coordinate system R1 B I BThat is, the rough stitching result of the point cloud of the object to be measured, the point cloud in the base coordinate system of the robot R1 I B Can be expressed as:

[0153]

[0154] Step S3.5: Obtain the rotation and translation matrix for the base coordinate system of the entity robot 2 to reach the base coordinate system of the entity robot 1 through rotation and translation

[0155] Step S3.5.1: First, fix a probe 1 and a probe 2 on the end flanges of the entity robot 1 and the entity robot 2 respectively, and set the TCP tool coordinate systems of the probe 1 and the probe 2 at the robot end;

[0156] After completing the setting of the TCP tool coordinate systems of the probe 1 and the probe 2, manually operate the entity robot 1 and the entity robot 2, point the probe 1 and the probe 2 at the same point in the working spaces of the two entity robots at the same time, record the pose P1 of the end of the probe 1 in the coordinate system of the entity robot 1 and the pose P2 of the end of the probe 2 in the coordinate system of the robot 2, and obtain the rotation and translation matrix H between the TCP coordinate system of the probe end and the base coordinate systems of the entity robots 1 and 2 according to the poses P1 and P2 g1 and H g2 , then Can be expressed as:

[0157]

[0158] Step S3.6: Obtain the point cloud of the object to be measured R2 I B Convert it to the coordinate system of the entity robot 1 to obtain the point cloud of the object to be measured R2 I′ B :

[0159]

[0160] In this way, the point cloud I of the object to be measured obtained by the dual robots is obtained B :

[0161] I B = R1 I B + R2 I′ B .

[0162] Example

[0163] In this example, the object to be measured is a cuboid with dimensions of 600mm * 450mm * 200mm. First, according to the actual measurement scenario, a 1:1 virtual simulation measurement environment is built on a computer equipped with the Ubuntu system and the ROS operating system. The virtual simulation measurement environment is as shown in Figure 4 . The structured light measuring instrument 1 and the structured light measuring instrument 2 are respectively installed on the flange plates at the ends of the six-degree-of-freedom robot 1 and the robot 2. The two robots are placed face to face, and the distance between the centers of the bases of the two robots is 3m. The object to be measured is placed on the measurement platform at the center of the two robots, 1.5m away. Secondly, the measurement path planning is carried out for the robot 1 and the robot 2 respectively according to the path planning method described in step S1. After all the measurement paths are set, the path information is sent to the physical six-degree-of-freedom robot.

[0164] In the actual measurement environment, the camera calibration and the hand-eye calibration are completed according to step S3.1, and the hand-eye matrix R1 H cg and R2 H cg are solved. And the relative pose calibration of the two robots is completed according to step S3.5, and the rotation and translation matrix from the base coordinate system of the physical robot 2 to the base coordinate system of the physical robot 1 is solved. After that, the six-degree-of-freedom physical robot 1 and the physical robot 2 respectively carry the structured light measuring instrument 1 and the structured light measuring instrument 2 and reach the set measurement points according to different paths for shooting. Figure 5 is the single-viewpoint cloud of the object to be measured obtained by the physical robot 1 carrying the structured light measuring instrument according to the planned measurement path under a single measurement plane. The single-viewpoint cloud on a single measurement plane is unified to the result under the base coordinate system of the physical robot 1 through coordinate transformation, as shown in Figure 6 . It can be seen that the single-viewpoint clouds on the same measurement plane can be successfully stitched. Figure 7 is the point cloud result under the base coordinate systems of the physical robot 1 and the physical robot 2. From Figure 7 (a), it can be seen that when the physical robot 1 takes pictures, the point cloud information on the back of the object to be measured cannot be obtained. From Figure 7 (b), it can be seen that when the physical robot 2 takes pictures, the point cloud information on the front of the object to be measured cannot be obtained. At this time, the point cloud of the object to be measured under the base coordinate system of a single robot is incomplete. Figure 7 is the final rough point cloud stitching result. It can be seen that the point clouds on different measurement planes can basically restore the three-dimensional shape of the object to be measured after rough point cloud stitching.

[0165] Although the above-described illustrative specific embodiments of the present invention have been described to facilitate understanding of the present invention by those skilled in the art, it should be clear that the present invention is not limited to the scope of the specific embodiments. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions made using the concept of the present invention are within the scope of protection.

Claims

1. A multi-robot collaborative three-dimensional measurement operation method, characterized by comprising: (1) Virtual simulation environment construction 1.1), According to the actual measurement environment, build a virtual simulation environment on a computer equipped with an open-source robot system, and import the description files of the dual robots into the virtual simulation environment to generate the corresponding Robot 1 and Robot 2. At the same time, in the virtual simulation environment, install the structured light measuring instrument 1 and the depth camera 1 on the fixed fixture of the end flange of Robot 1, and install the structured light measuring instrument 2 and the depth camera 2 on the fixed fixture of the end flange of Robot 2; 1.2), Simulate the object to be measured and place it between robots 1 and 2. Structured light measuring instrument 1 and structured light measuring instrument 2 are both facing the object to be measured directly. Determine multiple measurement planes for the structured light measuring instruments 1 and 2 to photograph the object to be measured and multiple measurement paths existing on each measurement plane. For structured light measuring instrument 1: The i-th measurement path on the k-th measurement plane is denoted as where K R1 is the number of measurement surfaces, M R1_k is the number of measurement paths on the k-th measurement surface. At the same time, each measurement path also contains multiple measurement points. The j-th measurement point in the i-th path on the k-th measurement surface of the object to be measured, that is, the measurement pose, is denoted as is the number of measurement poses on the measurement path of the k-th measurement surface. R1 represents robot 1. For the structured light measuring instrument 2: the i-th measurement path on the k-th measurement surface is denoted as where, K R2 is the number of measurement surfaces, M R2_k is the number of measurement paths on the k-th measurement surface. The j-th measurement point in the i-th path on the k-th measurement surface of the object to be measured, that is, the measurement pose, is denoted as is the number of measurement poses on the measurement path of the k-th measurement surface. R2 represents robot 2; The field of view ranges of the structured light measuring instruments 1 and 2 are both rectangles with a length of m centimeters and a width of n centimeters. For the structured light measuring instrument 1, the length of the circumscribed rectangle of the k-th measurement surface of the object to be measured is and the width is The measurement path is a series of longitudinal paths along the length direction, and the following constraints need to be satisfied: the number of measurement paths on the same measurement surface The number of measurement points on the same measurement path According to this constraint, the j-th measurement pose of the i-th measurement path of the k-th measurement surface is obtained For the structured light measuring instrument 2, the length of the circumscribed rectangle of the k-th measurement surface of the object to be measured is and the width is The measurement path is a series of longitudinal paths along the length direction, and the following constraints need to be satisfied: the number of measurement paths on the same measurement surface The number of measurement points on the same measurement path According to this constraint, the j-th measurement pose of the i-th measurement path of the k-th measurement surface is obtained (2), Measurement path planning 2.1) For the i-th measurement path on the k-th measurement surface of the robot 1 First, in the virtual simulation environment, drag the end of the robot to the measurement pose Among them, Measured pose is is the position coordinate of the end of robot 1, is the attitude coordinate of the end of robot 1; 2.2) At the measured pose of the robot 1 At this position, after using a depth camera to collect the image of the object to be measured, obtaining an RGB-D image and converting it into a grayscale image, threshold segmentation is performed to filter the background, and then the contour information of the object to be measured is extracted to obtain the minimum circumscribed rectangle where the contour of the object to be measured is located After that, in the range of the minimum circumscribed rectangle corresponding to the RGB-D image, the depth information d of all pixel points is traversed to find the shortest distance d between the object to be measured and the structured light measuring instrument min , and at the same time, record the pixel coordinates a(u, v) of the shortest distance, and then judge whether the shortest distance d min satisfies the measurement range of the structured light measuring instrument: d min ∈[D - δ, D + δ], where D is the focal length when the structured light measuring instrument performs camera calibration, and δ is the allowable error range of the structured light measuring instrument measurement; if it does not meet the requirement, go to step 2.3) to adjust the measurement pose, if it meets the requirement, go to step 2.4); 2.3), convert the pixel point coordinates a(u, v) through coordinate system transformation to obtain the corresponding space coordinates (x (u,v) , y (u,v) , z (u,v) ), and then combine with the position coordinates of the end of the robot 1 to determine a space straight line: Where x, y, and z are the coordinates on the space straight line; Then, starting from the position coordinates along this straight line in space, a position coordinate is calculated to satisfy the measurement condition: Obtain the position coordinates that meet the measurement conditions After that, combine with the original pose coordinates to form a measurement pose as the measurement pose According to the measurement pose Inverse kinematics of the robot is used to solve the corresponding joint states of the robot, and the pose adjustment is realized; 2.4), record the adjusted measured pose and return to step 2.1) to perform the next measured pose setting until the measurement path k S i for all measured poses k P ij the shortest distance d min has been judged. Then, proceed to step (3). Among them, before setting the next measured pose if: A. Measuring pose When the robot is set in a spatial pose where the degrees of freedom degenerate and the inverse kinematics has no solution, that is, near the singular point, a transition point needs to be set to ensure the smooth movement of the robot to avoid the singular point of the robot; B. When the shape of the object to be measured is irregular and there are protrusions on the surface of the object to be measured, in order to obtain better measurement results and avoid collisions with the irregular object to be measured during the movement of the robot carrying the structured light measuring instrument, when setting the measurement pose it is necessary to consider the actual measurement pose of the robot and the morphological characteristics of the object to be measured. Between the measurement pose and the measurement pose a transition point Transition point No distance detection and adjustment are required at the transition point which is used to artificially adjust the measurement path of the robot; 2.5), evaluate the measurement path for path evaluation All measured poses and all transition points constitute a complete measurement path For the measurement path perform path evaluation as follows: Step 2.5.1), execute the measurement path in the virtual simulation environment The robot 1 will move from the measurement pose continuously to the measurement pose Record the process points by equally time sampling during the movement t is the moment, T is the moment of the movement end point, the set constitutes the actual movement path of the robot as In the measurement path where the starting position coordinates are and the ending position coordinates are The shortest distance between the ending position and the starting position is: In the actual motion path Among them, its starting position coordinates are The ending position coordinates are Then the actual motion length of the robot is: Subtracting the two equations gives: L＝l2-l1 Then the evaluation function f1(L) is expressed as: f1(L) = (δ1 - L) / δ1, 0 ≤ L ≤ δ1, where δ1 is the set maximum error threshold; Step 2.5.2), find a pose in the actual motion path of the robot 1 such that the distance l3 from this pose to the centroid coordinates (Px obj , Py obj , Pz obj ) of the measured object is minimized. Then the distance l3 is expressed as: Among them, is the pose position coordinates; Then the evaluation function f2(l3) is expressed as: f2(l3) = (l3 - δ2) / l3 Where δ2 is the minimum distance to ensure that the structured light measuring instrument does not collide with the object to be measured; 2.5.3) For the measurement path The comprehensive evaluation function k F i is as follows: According to the comprehensive evaluation function k F i evaluate the set measurement path. When the comprehensive evaluation function is passed, proceed to step 2.7); otherwise, proceed to step 2.6), where g is a threshold value defined manually by the measurement personnel according to different measurement scenarios, and 0 < g < 100; 2.6) Traverse all poses in to find the pose closest to the pose . After that, move the robot to the measurement pose . Drag the end of the robot to manually increase the distance d between the current measurement pose of the robot and the object to be measured . If the pose min is the measurement pose, the adjustment range still needs to meet the measurement conditions of the structured light measuring instrument: d ∈[D - δ, D + δ]. If the pose min is the transition point, the distance d needs to be adjusted according to the setting conditions of the transition point . The adjusted pose replaces the pose min to complete the correction of the measurement path . Then enter step 2.7);​ 2.7), Process all the measurement paths of all the measurement surfaces according to steps 2.1) to 2.6) to complete the simulation of the measurement path planning. Convert the planned measurement path into a communication message that the robot can recognize and send it from the virtual simulation environment to the physical Robot 1; For robot 2, according to the method in steps 2.1)-2.7), obtain the planned measurement path and convert it into a communication message recognizable by the robot and send it to the physical robot 2 by the virtual simulation environment; (3), Real environment measurement 3.1), Perform hand-eye calibration on the physical robot 1 to obtain the rotation and translation matrix of the camera coordinate system relative to the end flange coordinate system of the physical robot 1, that is, the hand-eye relationship matrix R1 H cg ; 3.2) After completing the hand-eye calibration, start measuring the object to be measured. After the physical robot 1 receives the measurement path sent by the virtual simulation environment, it moves to the measurement pose in sequence ; 3.3), when reaching the measurement pose the physical robot 1 sends the measurement pose to the structured light measuring instrument, and obtains the single-viewpoint cloud of the measured object at the current measurement pose through the binocular vision camera and uses the hand-eye relationship matrix R1 H cg and the pose matrix from the end flange coordinate system of the physical robot 1 to the base coordinate system of the physical robot 1 obtained according to the measurement pose to transform the single-viewpoint cloud to the base coordinate system of the physical robot 1; In this way, the point cloud of the object to be measured in the base coordinate system of the entity robot 1 is obtained. R1 I B That is, the rough stitching result of the point cloud of the object to be measured; 3.4) For the physical robot 2, according to the method in steps 3.1)-3.3), obtain the point cloud of the measured object in the base coordinate system of the physical robot 1 R2 I B That is, the rough stitching result of the point cloud of the measured object 3.5), obtain the rotation and translation matrix of the base coordinate system of the entity robot 2 to reach the base coordinate system of the entity robot 1 through rotation and translation 3.5.1), First, fix a probe 1 and a probe 2 on the end flanges of the physical Robot 1 and the physical Robot 2 respectively, and set the TCP tool coordinate systems of the probe 1 and the probe 2 at the robot end; 3.5.2) After setting the TCP tool coordinate systems of probe 1 and probe 2, manually operate entity robot 1 and entity robot 2 to simultaneously point probe 1 and probe 2 at the same point within the working spaces of the two entity robots. Record the pose P1 of the end of probe 1 in the coordinate system of entity robot 1 and the pose P2 of the end of probe 2 in the coordinate system of robot 2. Obtain the rotation and translation matrix H between the TCP coordinate system of the probe end and the base coordinate systems of entity robots 1 and 2 based on poses P1 and P2 g1 and H g2 , then can be expressed as: 3.6), Obtain the point cloud of the object to be measured R2 I B Convert it to the coordinate system of the entity robot 1 to obtain the point cloud of the object to be measured R2 I′ B : In this way, the point cloud I obtained by the double robots for the object to be measured is obtained B : I B = R1 I B + R2 I′ B .

Images