A method and system for robots to perform assembly tasks based on 3D vision

Abstract

This invention relates to the field of automated robot assembly technology, specifically a method and system for robot assembly tasks based on 3D vision. The method includes the following steps: Step 1: Workpiece surface scanning: Scan the workpiece surface, construct a 3D model of the workpiece surface, determine each measurement point n, and construct a point cloud based on the depth measurement values ​​of the measurement points; For the first time, the ICP algorithm is used to solve the robot assembly task by employing a low-cost sensor, that is, the cost function is iteratively minimized based on the estimation of the point correspondence between two point clouds, making the process robust to incorrect geometric alignment. In the subsequent search phase, the Lissajous function is used for surface sliding and admittance control, enabling the robot to complete the assembly task under the condition of random position errors.

Description

Technical Field

[0001] This invention relates to the field of automated robot assembly technology, specifically a method and system for robots to perform assembly tasks based on 3D vision. Background Technology

[0002] The current trend in industrial manufacturing is to use more autonomous and efficient robots to quickly adapt to changes in production structures and components, while ensuring a certain level of safety and robustness. In assembly tasks, the most important task is drilling. Taking the task of inserting steel bolts into carbon fiber workpieces in the production of new energy vehicles as an example, currently, this is performed manually by operators. Operators use appropriately machined caps / clamps to insert bolts to determine the correct position of the hole for different types of bolts. Manual drilling is inefficient and prone to errors. Using robots to complete this task is the mainstream approach. In the process, the posture of the target object relative to the robot is known, but the uncertainties such as the hole position and tilt angle are far greater than the tolerance of the task.

[0003] Therefore, there is an urgent need to invent a method and system for robots to perform assembly tasks based on 3D vision to solve the above problems. Summary of the Invention

[0004] To address the above problems, the present invention provides the following technical solution: a method for a robot to perform assembly tasks based on 3D vision, the method comprising the following steps;

[0005] Step 1: Workpiece surface scanning: Scan the workpiece surface, construct a 3D model of the workpiece surface, determine each measurement point n, and construct a point cloud based on the depth measurement values ​​of the measurement points;

[0006] Step 2: Surface Reconstruction: Use the point-to-plane ICP algorithm in the most recent iteration of the ICP registration algorithm to align the acquired point cloud and reconstruct the workpiece surface;

[0007] Step 3: Hole localization: Compare the obtained point cloud with the known point cloud model, calculate a local feature vector called the fast point feature histogram, and then use the Random Sample Consensus (RANSAC) algorithm to find the corresponding points of the two point clouds and detect the hole positions and their unit normal vectors on the reconstructed surface.

[0008] Step 4: Approach Phase: Align the nail with the hole axis based on the closed-loop inverse kinematics algorithm, and move the nail to a position close to the workpiece surface;

[0009] Step 5: Search Phase: The nail tip explores the vicinity of the hole by sliding along the trajectory described by the Lissajous function on the surface;

[0010] Step Six: Insertion Phase: When the nail tip finds the hole, the nail is immediately inserted about 1 mm and the search phase stops. An admittance control scheme is used to compensate for small directional errors between the nail and the hole normal.

[0011] Preferably, in step two above, the point-to-point ICP algorithm has a faster convergence speed for any pair of corresponding points of the same point on the surface. There exists a unique homogeneous transformation matrix. Then, the two point clouds S and Q obtained from the same surface in two different views are in a registered state, as shown in the following formula, where the symbols are... and Represents the homogeneous representation of a coordinate vector;

[0012] ;

[0013] Furthermore, a set of homogeneous transformation matrices needs to be determined. To align the point cloud obtained from n in the global coordinate system, where si and qi are the points within it, and T is the homogeneous transformation matrix. and The homogeneous representation of the coordinate vectors si and qi are points within it, and T is the homogeneous transformation matrix. and Homogeneous representation of coordinate vectors.

[0014] Preferably, the above matrix Using the ICP algorithm, a set of control points is selected in the point cloud, and the corresponding normals are... , Initialize to the identity matrix, and compute the minimum cost function in each iteration until convergence error. Below the tolerance threshold The cost function and convergence error are calculated as follows: [The process stops at this point].

[0015] ;

[0016] ;

[0017] Cm represents the cost function; let's keep that consistent here. I(4×4) is a 4×4 identity matrix.

[0018] Preferably, the above The matrix that minimizes the cost function is found by using least squares estimation. The cost function is transformed into the following equation, where It is a point cloud At point The homogeneous representation of the unit vector perpendicular to the surface, as shown in the figure;

[0019] .

[0020] Preferably, in step three above, the transformation matrix between point clouds obtained by the RANSAC algorithm is used as the initial guess of the ICP algorithm responsible for refinement and alignment.

[0021] Preferably, when a mismatch occurs in step three above, i.e. a connection between two points in the point cloud that do not actually correspond, the mismatch is deleted using a pruning algorithm.

[0022] Preferably, in step four above, the alignment task is characterized by a task function and its corresponding Jacobian matrix formula, as shown below, where It is the unit vector of the end effector reference frame aligned with the tip of the pin. Yes, it performs the skew-symmetric matrix operation of cross-product;

[0023] ;

[0024] .

[0025] Preferably, in step four above, the nail tip moves to the target point, which is located at a given distance from the surface and belongs to the axis. The point on the map. The corresponding task function is the position of the nail. The Jacobian matrix is The joint calculation of the two tasks is as follows;

[0026] ;

[0027] in Denotes the right pseudo-inverse of a matrix. and This is the reference position for the nail. and These are the positive definite scalar and matrix gain, respectively, and the current position of the Pe nail. It is the task function for the calibration task, and what is obtained is the calibration deviation.

[0028] Preferably, in step five above, the calculation formula for the area adjacent to the exploration hole is as follows, where , , and These are the amplitude and frequency of the sine wave, respectively. It is the moment when the tip contacts the surface, and the superscript 𝑒 represents the trajectory in the coordinate system. The representation in;

[0029] .

[0030] A robot assembly task system based on 3D vision includes a robot, a vision system, a controller, and a computing module.

[0031] The robot is used to move a vision system along a planned path across its workspace and to perform assembly tasks.

[0032] The vision system includes: a depth sensing camera and a volume element grid filter, used to scan the surface of the target object;

[0033] The calculation module is used to calculate the information scanned by the vision system.

[0034] The controller is used to control the robot to perform mobile assembly tasks.

[0035] The technical effects and advantages of this invention are as follows:

[0036] This invention is the first to solve robot assembly tasks by using the ICP algorithm with low-cost sensors. Specifically, it iteratively minimizes the cost function based on the estimation of the point correspondence between two point clouds, making the process robust to incorrect geometric alignment. In the subsequent search phase, the Lissajous function enables the robot to complete the assembly task even with random position errors through surface sliding and admittance control.

[0037] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures pointed out in the description and the drawings. Attached Figure Description

[0038] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0039] Figure 1 This is a flowchart illustrating the present invention;

[0040] Figure 2 This is a timing diagram of the present invention;

[0041] Figure 3 This is a schematic diagram of the hole positioning algorithm in this invention. Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0043] In the description of this invention, it should be understood that the terms "length," "width," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicating orientation or positional relationships, are based on the orientation or positional relationships shown in the accompanying drawings and are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention. Furthermore, in the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0044] like Figures 1 to 3 As shown; a method for a robot to perform assembly tasks based on 3D vision, the method includes the following steps;

[0045] Step 1: Workpiece surface scanning: Scan the workpiece surface, construct a 3D model of the workpiece surface, determine each measurement point n, and construct a point cloud based on the depth measurement values ​​of the measurement points;

[0046] Step 2: Surface Reconstruction: Use the point-to-plane ICP algorithm in the most recent iteration of the ICP registration algorithm to align the acquired point cloud and reconstruct the workpiece surface;

[0047] Step 3: Hole localization: Compare the obtained point cloud with the known point cloud model, calculate a local feature vector called the fast point feature histogram, and then use the Random Sample Consensus (RANSAC) algorithm to find the corresponding points of the two point clouds and detect the hole positions and their unit normal vectors on the reconstructed surface.

[0048] Step 4: Approach Phase: Align the nail with the hole axis based on the closed-loop inverse kinematics algorithm, and move the nail to a position close to the workpiece surface;

[0049] Step 5: Search Phase: The nail tip explores the vicinity of the hole by sliding along the trajectory described by the Lissajous function on the surface;

[0050] Step Six: Insertion Phase: When the nail tip finds the hole, the nail is immediately inserted about 1 mm and the search phase is stopped. An admittance control scheme is used to compensate for small directional errors between the nail and the hole normal.

[0051] Surface scanning assumes that the robot control unit roughly knows the geometry of the workpiece and its position and orientation relative to the robot's base frame. A depth camera mounted on the robot's end effector scans the workpiece surface, constructs a 3D model of the surface, and determines each measurement point n corresponding to the end effector's orientation based on the surface geometry and orientation, as well as available information such as camera range and field of view, thereby covering the entire surface. Then, the robot moves over each measurement point and constructs a point cloud based on the acquired depth measurements.

[0052] Surface reconstruction uses the point-to-plane ICP algorithm in the nearest neighbor iterative (ICP) registration algorithm to align the acquired point cloud and reconstruct the workpiece surface because it has a faster convergence speed than the point-to-point ICP algorithm. If for any pair of corresponding points of the same point on the surface... There exists a unique homogeneous transformation matrix. Then, the two point clouds S and Q obtained from the same surface in two different views are in a registered state, as shown in the following formula, where the symbols are... and This represents the homogeneous representation of the coordinate vector.

[0053] ;

[0054] Next, a set of homogeneous transformation matrices needs to be determined. , so as to align the point clouds obtained by n in the global coordinate system, each point cloud being composed of Composed of points, The coordinate system representing the camera. This corresponds to the i-th measurement point.

[0055] The matrix is ​​calculated using the following process. For each , calculation will arrive Aligned transformation matrix First, the point cloud is downsampled using a volume element mesh filter. Then, the ICP algorithm is used to select a set of control points in the point cloud, with corresponding normals as follows. , Initialize to the identity matrix, and compute the minimum cost function in each iteration until convergence error. Below the tolerance threshold The process stops at a certain point, and the formulas for calculating the cost function and convergence error are shown below.

[0056] ;

[0057] ;

[0058] because With 12 unknown components, by considering at least 4 pairs of corresponding points, the matrix minimizing the cost function can be found using least squares estimation. In a point-to-plane ICP, the surface is represented by a point cloud. The tangent plane can be locally approximated, transforming the cost function into the following equation, where... It is a point cloud At point The homogeneous representation of the unit vector perpendicular to the surface is shown in the figure.

[0059] ;

[0060] Once the point clouds are aligned (n), they can be merged into a single point cloud. Then, a volumetric element grid filter is used to downsample the point clouds, and the resulting point cloud is obtained. This represents the reconstruction result of the entire workpiece surface in the coordinate system.

[0061] ;

[0062] Where Ti is the homogeneous transformation matrix, n is the number of point clouds, and Ni is the number of constituent points in the current point cloud. It refers to the points in a point cloud.

[0063] Hole positioning is the process of obtaining point clouds Compared with known point cloud models, for For each point, a local feature vector, called a Fast Point Feature Histogram (FPFH), is calculated. Then, the Random Sample Consensus (RANSAC) algorithm is used to find corresponding points between two point clouds. In each iteration, from... Randomly selected The point is then used to determine the nearest point based on the extracted features. The corresponding point. Because it is obtained through the RANSAC algorithm. and The accuracy of the transformation matrix between them is not very high, so it is used as an initial guess for the ICP algorithm responsible for thinning and alignment, the center of each hole. The position and its corresponding axis, i.e., the unit vector perpendicular to the surface. It is known in the point cloud and can be found in the reconstructed point cloud. The process involves estimating the position of each hole in the original point cloud and locating it in a coordinate system. Finally, the actuator transformation at the camera end is determined through camera calibration, and the holes are then located in the robot's base coordinate system. During this process, mismatches may occur—connections between two points in the point cloud that do not actually correspond. These are removed using a pruning algorithm.

[0064] The robot's movement toward the hole is controlled by a closed-loop inverse kinematics algorithm, which has two tasks: first, to align the nail with the hole axis, and second, to move the nail to a position close to the surface.

[0065] The alignment task is characterized by its task function and its corresponding Jacobian matrix formula, as shown below, where It is the unit vector of the end effector reference frame aligned with the tip of the pin. It performs oblique symmetric matrix operations that perform cross-products.

[0066] ;

[0067] ;

[0068] The goal of the position tracking task is to move the spike tip from its current position to a target point, which is located at a given distance from the surface and belongs to the axis. The point on the map. The corresponding task function is the position of the nail. The Jacobian matrix is The joint calculation of the two tasks is as follows.

[0069] ;

[0070] in Denotes the right pseudo-inverse of a matrix. and This is the reference position for the nail. and These are positive definite scalar and matrix gain, respectively.

[0071] The accuracy of hole location estimation depends on the error in surface reconstruction. If this error exceeds the hole gap, pin insertion cannot be guaranteed, hence the introduction of a search phase. The search phase begins when contact is detected along a direction perpendicular to the reconstructed surface. This is achieved by following the path described by the Lissajous function. The path in the plane is searched by sliding the tip of the pin on the workpiece surface, as shown in the following formula, where... , , and These are the amplitude and frequency of the sine wave, respectively. It is the moment when the tip contacts the surface, and the superscript 𝑒 represents the trajectory in the coordinate system. The representation in the text.

[0072] ;

[0073] To reduce mechanical stress on the workpiece and robot joints, and to allow the nail to slide more smoothly, enabling the nail tip to insert more easily into the hole, an admittance control scheme is proposed to apply impedance behavior. Assume the nail tip's posture is... The calculation is as follows.

[0074] ;

[0075] ;

[0076] in , and These are the virtual inertia, damping, and stiffness matrices applied to the nail, respectively. (Vector) It is the relative displacement between the desired pose and the reference pose. It is a matrix that maps the time derivative of Euler angles to angular velocities. and These are the 3×3 identity matrix and the zero matrix, respectively. 端执行器上的扳手的力，TA是个扩充矩阵。

[0077] ；

[0078] 其中 和 分别是期望位置和参考位置，而 是在期望坐标系 与参考坐标系 间表示相对方位的欧拉角的向量；

[0079] 由于在搜索阶段，计划轨迹选择在孔表面下方几毫米处，以便允许沿 轴产生耦合力，因此当钉尖端检测到孔时，钉立即插入约1毫米并停止搜索阶段。从该钉的位置开始，沿着估计孔的轴规划一条新轨迹，以便执行插入阶段。在插入过程中，使用导纳控制方案以补偿钉子和孔法线之间的小方向误差。

[0080] 尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims

1. A method for a robot to perform assembly tasks based on 3D vision, characterized in that: The method includes the following steps; Step 1: Workpiece surface scanning: Scan the workpiece surface, construct a 3D model of the workpiece surface, determine each measurement point n, and construct a point cloud based on the depth measurement values ​​of the measurement points; Step 2: Surface Reconstruction: Use the point-to-plane ICP algorithm in the most recent iteration of the ICP registration algorithm to align the acquired point cloud and reconstruct the workpiece surface; Step 3: Hole localization: Compare the obtained point cloud with the known point cloud model, calculate a local feature vector called the fast point feature histogram, and then use the Random Sample Consensus (RANSAC) algorithm to find corresponding points between the two point clouds. Detect the hole positions and their unit normal vectors on the reconstructed surface. Use the transformation matrix between the point clouds obtained by the RANSAC algorithm as the initial guess of the ICP algorithm responsible for thinning and alignment. When a mismatch occurs, i.e., a connection between two points in the point cloud that do not actually correspond, it is deleted using a pruning algorithm. Step 4: Approach Phase: Based on the closed-loop inverse kinematics algorithm, the nail is aligned with the hole axis, and the nail is moved to a position close to the workpiece surface. The alignment task is characterized by the task function and its corresponding Jacobian matrix formula, as shown below, where... It is the unit vector of the end effector reference frame aligned with the tip of the pin. It performs skew-symmetric matrix operations that involve cross-products; ； ； Where Ja is the Jacobian matrix for the alignment task; When the nail tip moves to the target point, which is at a given distance from the surface and belongs to the axis... The point on the map corresponds to the position of the nail in the task function. The Jacobian matrix is The joint calculation of the two tasks is as follows; ； in Denotes the right pseudo-inverse of a matrix. and This is the reference position for the nail. and These are the positive definite scalar and matrix gain, respectively. Ja is the Jacobian matrix for the alignment task, and Pe is the current position of the nail. It is the task function for the calibration task; Step 5: Search Phase: The nail tip explores the vicinity of the hole by sliding along the trajectory described by the Lissajous function on the surface; Step Six: Insertion Phase: When the nail tip finds the hole, the nail is immediately inserted about 1 mm and the search phase stops. An admittance control scheme is used to compensate for small directional errors between the nail and the hole normal.

2. The method for a robot to perform assembly tasks based on 3D vision according to claim 1, characterized in that: In step two above, the point-to-point ICP algorithm has a faster convergence speed for any pair of corresponding points on the same point on the surface. There exists a unique homogeneous transformation matrix. Then, the two point clouds S and Q obtained from the same surface in two different views are in a registered state, as shown in the following formula, where the symbols are... and Represents the homogeneous representation of a coordinate vector; ； Furthermore, a set of homogeneous transformation matrices needs to be determined. This is to align the point cloud obtained by n in the global coordinate system.

3. The method for a robot to perform assembly tasks based on 3D vision according to claim 1, characterized in that: In step five above, the calculation formula for the area adjacent to the exploration hole is as follows: , It is the amplitude of a sine wave. and It's frequency. It is the moment when the tip contacts the surface, and the superscript 𝑒 represents the trajectory in the coordinate system. The representation in; 。 4. A system for performing assembly tasks for a 3D vision-based robot as described in any one of claims 1-3, characterized in that: This includes robots, vision systems, controllers, and computing modules; The robot is used to move a vision system along a planned path across its workspace and to perform assembly tasks. The vision system includes: a depth sensing camera and a volume element grid filter, used to scan the surface of the target object; The calculation module is used to calculate the information scanned by the vision system. The controller is used to control the robot to perform mobile assembly tasks.

Images