Bolt 6D pose detection and visual servo method based on 3D template matching

Abstract

The invention discloses a 6D pose detection and visual servo method of a bolt based on 3D template matching. The method comprises the following steps: S1, establishing a CAD model of the bolt; s2, acquiring a bolt image, and performing image preprocessing; s3, performing bolt feature information detection to obtain feature points, feature lines and feature surfaces; s4, bolt initial point matching is carried out, and matched feature information is stored; s5, performing tracking and 6D pose estimation on the bolt, and performing subtraction on the estimated 6D pose and a set expected pose as input of a visual servo algorithm; s6, acquiring the movement speed of the depth camera; s7, the movement speed of the mechanical arm is obtained and sent to the mechanical arm, and the mechanical arm is made to move; s8, the steps S5-S8 are repeated until the pose error in the step S5 is smaller than the pose error threshold value; and S9, a move L instruction is used for enabling an end effector installed at the tail end of the mechanical arm to conduct bolt alignment. According to the invention, the bolt can be stably tracked and the 6D pose of the bolt can be detected in real time.

Description

Technical Field

[0001] The present invention relates to the field of machine vision technology, and in particular to a 6D position detection and visual servo method of a bolt based on 3D template matching. Background Art

[0002] With the continuous development of railway transportation network, the stability and safety of the contact network, an important component of the electrified railway system, are of vital importance. Due to natural environmental factors or operational wear and tear, the various structures of the contact network may be deformed or damaged to a certain extent. These factors pose a potential threat to the normal operation of the contact network, especially the bolts on the contact network connection parts. Contact network bolts are one of the key elements for fixing contact network components. In the contact network system, the number of bolts is huge and widely distributed, so the identification and maintenance of bolts are of great significance.

[0003] The traditional method of catenary bolt inspection and maintenance mainly relies on manual labor, which is not only inefficient and easily affected by subjective factors, but also the quality of workers' work and personal safety during high-altitude operations cannot be guaranteed, so it is difficult to meet the needs of large-scale and high-frequency inspections. In order to solve this problem, an important method is to introduce a robotic arm, which can provide high efficiency for the inspection and maintenance of catenary bolts, while reducing the operating risks and labor intensity of maintenance workers.

[0004] The robotic arm can replace manual labor. It can identify the bolts on the contact network components through the camera carried on it, and then guide the tool carried on the end of the robotic arm to move to the target bolt to perform tightening operations. Compared with manual labor, it has the advantages of high work efficiency, short operation time and high work quality.

[0005] During the operation of the robot arm, it is particularly important to obtain the 6D pose of the target bolt. Since the bolt is hexagonal, the target volume is relatively small and there is no texture information. The color of the bolt is similar to the color of the contact network pole, and there are problems such as part occlusion and insufficient light. How to accurately estimate the bolt pose and correctly guide the robot arm is difficult. The existing detection method mainly detects the position of the bolt and cannot accurately detect the posture information of the bolt. During the movement of the robot arm, the change of posture will cause the final bolt alignment to be unable to align. Summary of the invention

[0006] In view of the above problems, the present invention aims to provide a 6D pose detection and visual servoing method of bolts based on 3D template matching.

[0007] The technical solution of the present invention is as follows:

[0008] A 6D position detection and visual servoing method of a bolt based on 3D template matching comprises the following steps:

[0009] S1: Acquire an actual image of a target bolt, and establish a CAD model of the bolt according to the actual image;

[0010] S2: photographing a target bolt using a depth camera installed at the end of the robotic arm to obtain a bolt image, and performing image preprocessing on the bolt image;

[0011] S3: Detect bolt feature information according to the bolt image after image preprocessing to obtain feature points, feature lines and feature surfaces;

[0012] S4: matching the initial point of the CAD model in step S1 with the initial point of the bolt obtained by detecting the bolt feature information in step S3, and storing the matched feature information;

[0013] S5: Tracking and estimating the 6D pose of the bolt, and subtracting the estimated 6D pose from the set desired pose, and using the subtraction result as the input of the visual servo algorithm;

[0014] S6: Calculate the movement speed of the depth camera according to the posture error of step S5;

[0015] S7: obtaining the movement speed of the robotic arm through inverse kinematics according to the movement speed of the depth camera, and sending the movement speed of the robotic arm to the robotic arm to make the robotic arm move;

[0016] S8: repeating steps S5-S8 until the posture error of step S5 is less than the posture error threshold, at which time the visual servoing is completed, the robot arm stops moving, and the posture of the depth camera is parallel to the bolt;

[0017] S9: Use the move L command to align the bolts of the end effector mounted at the end of the robot arm.

[0018] Preferably, in step S1, after the CAD model of the bolt is established, it is saved in CAD format.

[0019] Preferably, in step S2, the image preprocessing includes image segmentation processing and image grayscale processing.

[0020] Preferably, Gamma transformation is used to perform image grayscale processing.

[0021] Preferably, in step S5, when the bolt is tracked and the 6D pose is estimated, the information of the characteristic points and characteristic lines of the bolt is tracked, and the bolt is tracked and the pose is estimated by using a virtual visual servoing method.

[0022] Preferably, the virtual visual servoing method is:

[0023] Visual servo controls a virtual camera and a real camera, wherein the virtual camera and the real camera respectively obtain images of a target object;

[0024] The edge method is used to track the edge line. During tracking, the visual feature p is composed of a set of distances d ⊥ Composition, the local point feature p obtained by the tracker t The distance between the contour of the object L, at this time the expected eigenvalue p* = 0; assuming that the contour of the target object can be described by straight lines or smooth curves, all distances are processed according to their corresponding line segments or ellipses;

[0025] The alignment process of the projection features of the virtual camera and the real features of the real camera is the overlapping process of the virtual camera and the real camera, which is expressed by minimizing the error function:

[0026]

[0027] Where: Δ is the minimized error; n is the number of local feature points; p t i is the current local feature point; According to the current pose parameter μ t projection of the calculated three-dimensional model into the image;

[0028] By minimizing the observed data p* and calculating the position of the same feature according to the current pose through forward projection The current bolt position is estimated by minimizing the error between:

[0029]

[0030] Where: pr ξ (μ, ω P i ) is the projection model determined according to the camera intrinsic parameter ξ and camera pose μ; p *i To minimize the observed data;

[0031] At convergence, the pose that minimizes the error is assumed to be the true pose;

[0032] The interaction matrix between the distance change between the fixed point and the moving line and the virtual camera movement is:

[0033]

[0034] λ ρ =(Aρcosθ+Bρsinθ+C) / D (7)

[0035] λ θ =(Asinθ-Bcosθ) / D (8)

[0036] α=x d sinθ-y d cosθ (9)

[0037] Where: J d is the interaction matrix; are the interaction matrices of different edge points respectively; · represents transpose; N is the number of edge points of the object; is the straight line d l The Jacobian matrix of ; λ ρ , θ , α are intermediate parameters; θ is the angle of polar coordinates; ρ is the distance from the origin of polar coordinates to the straight line; A, B, C, D are coefficients; x d and d are the coordinates of the tracked point.

[0038] Preferably, in step S6, the movement speed of the depth camera is obtained by the following formula:

[0039]

[0040] Where: v c is the speed of the depth camera; λ is a constant; is the expected position information of the target in the camera coordinate system; c t o is the position information of the target in the camera coordinate system; θu is the posture information of the target.

[0041] The beneficial effects of the present invention are:

[0042] The present invention can stably track bolts and detect the 6D posture of the bolts in real time. Specifically, the 6D posture information of the bolts is obtained by adopting a 3D template matching method, and the posture of the end effector installed at the end of the robot arm is adjusted by a visual servo method, thereby achieving the bolt tightening task. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0044] Figure 1 It is a flow chart of the 6D posture detection and visual servoing method of bolts based on 3D template matching of the present invention;

[0045] Figure 2 1 is a physical schematic diagram of an M12 bolt in a specific embodiment; wherein 1, 2, 3, and 4 are four initial characteristic points of the M12 bolt respectively;

[0046] Figure 3 for Figure 2 Schematic diagram of the CAD model of the M12 bolt; 1, 2, 3, and 4 are the four initial feature points of the M12 bolt respectively;

[0047] Figure 4 Schematic diagram of image segmentation processing;

[0048] Figure 5 It is a schematic diagram of characteristic detection results of an M12 bolt in a specific embodiment;

[0049] Figure 6 is a schematic diagram of a virtual visual servoing method;

[0050] Figure 7 It is a schematic diagram of the distance from a point to a straight line;

[0051] Figure 8 Schematic diagram of the linear velocity during the visual servoing process of the robot arm. DETAILED DESCRIPTION

[0052] The present invention is further described below in conjunction with the accompanying drawings and embodiments. It should be noted that, in the absence of conflict, the embodiments in this application and the technical features in the embodiments can be combined with each other. It should be noted that, unless otherwise specified, all technical and scientific terms used in this application have the same meanings as those generally understood by those of ordinary skill in the art to which this application belongs. The words "including" or "comprising" and the like used in the disclosure of the present invention mean that the elements or objects appearing before the word cover the elements or objects listed after the word and their equivalents, without excluding other elements or objects.

[0053] like Figure 1 As shown, the present invention provides a 6D posture detection and visual servoing method of a bolt based on 3D template matching, comprising the following steps:

[0054] S1: Acquire an actual image of a target bolt, and establish a CAD model of the bolt according to the actual image.

[0055] In a specific embodiment, after the CAD model of the bolt is established, it is saved in CAD format and used as input for the tracked bolt CAD model.

[0056] In a specific embodiment, taking an M12 bolt as an example, the actual bolt image is as follows: Figure 2 As shown in Figure 2, the established CAD model is as follows: Figure 3As shown in the figure, the CAO format file for importing M12 bolts is:

[0057] #Blender v2.78(sub 0)CAO File

[0058] V1

[0059] #3D points 12 0.011000 0.000000 0.003500

[0062] 0.011000 0.000000 -0.003500 0.005500 0.008660 0.003500

[0064] 0.005500 0.008660 -0.003500

[0065] -0.005500 0.008660 0.003500

[0066] -0.005500 0.008660 -0.003500

[0067] -0.011000 0.000000 0.003500

[0068] -0.011000 0.000000 -0.003500

[0069] -0.005500 -0.008660 0.003500

[0070] -0.005500 -0.008660 -0.003500

[0071] 0.005500 -0.008660 0.003500

[0072] 0.005500 -0.008660 -0.003500

[0073] #3D lines 0

[0075] #Faces from 3D lines 0

[0077] #Faces from 3D points 8 4 1 0 10 11 4 0 1 3 2 4 2 3 5 4 4 4 5 7 6 4 6 7 9 8 4 8 9 11 10 6 9 7 5 3 1 11 6 6 8 10 0 2 4

[0087] #3D cylinders 0

[0089] #3D circles 0

[0091] The files of the four initial feature points are:

[0092]

[0093] S2: Use a depth camera installed at the end of the robotic arm to shoot the target bolt to obtain a bolt image, and perform image preprocessing on the bolt image.

[0094] In a specific embodiment, the image preprocessing includes image segmentation processing and image grayscale processing. Optionally, the schematic diagram of the image segmentation processing is as follows: Figure 4 As shown, Gamma transformation is used to process the image grayscale.

[0095] S3: Detect bolt feature information based on the bolt image after image preprocessing to obtain feature points, feature lines and feature surfaces.

[0096] In a specific embodiment, the feature points are represented by red, the feature lines are represented by green, and the detection results are as follows: Figure 5 shown.

[0097] S4: Matching the initial point of the CAD model in step S1 with the initial point of the bolt obtained by detecting the bolt feature information in step S3, and storing the matched feature information.

[0098] In a specific embodiment, when matching, that is, matching Figure 2 and Figure 3 The four initial feature points are shown as 1, 2, 3, and 4. When storing the matched feature information, the stored information is the description of the feature information of the bolt. The data with better matching results are manually selected for storage, so that there is a data set with good data quality when the bolt is tracked later.

[0099] S5: Track the bolt and estimate its 6D pose, and subtract the estimated 6D pose from the set desired pose, using the difference result as the input of the visual servo algorithm.

[0100] In a specific embodiment, when tracking and estimating the 6D position and posture of the bolt, the information of the characteristic points and characteristic lines of the bolt is tracked, and the bolt is tracked and the position and posture are estimated by using a virtual visual servoing method. Figure 6-7 As shown, the virtual visual servoing method is:

[0101] Visual servo controls a virtual camera and a real camera, wherein the virtual camera and the real camera respectively obtain images of a target object;

[0102] The edge method is used to track the edge line. During tracking, the visual feature p is composed of a set of distances d ⊥ Composition, the local point feature p obtained by the tracker t The distance between the contour of the object L, at this time the expected eigenvalue p* = 0; assuming that the contour of the target object can be described by straight lines or smooth curves, all distances are processed according to their corresponding line segments or ellipses;

[0103] The alignment process of the projection features of the virtual camera and the real features of the real camera is the overlapping process of the virtual camera and the real camera, which is expressed by minimizing the error function:

[0104]

[0105] Where: Δ is the minimized error; n is the number of local feature points; p t i is the current local feature point; According to the current pose parameter μ t projection of the calculated three-dimensional model into the image;

[0106] By minimizing the observed data p* and calculating the position of the same feature according to the current pose through forward projection The current bolt position is estimated by minimizing the error between:

[0107]

[0108] Where: pr ξ (μ, ω P i ) is the projection model determined according to the camera intrinsic parameter ξ and camera pose μ; p *i To minimize the observed data;

[0109] At convergence, the pose that minimizes the error is assumed to be the true pose;

[0110] The interaction matrix between the distance change between the fixed point and the moving line and the virtual camera movement is:

[0111]

[0112]

[0113] λ ρ =(Aρcosθ+Bρsinθ+C) / D (7)

[0114] λ θ =(Asinθ-Bcosθ) / D (8)

[0115] α=x d sinθ-y d cosθ (9)

[0116] Where: J d is the interaction matrix; are the interaction matrices of different edge points respectively; · represents transpose; N is the number of edge points of the object; is the straight line d l The Jacobian matrix of ; λ ρ , θ , α are intermediate parameters; θ is the angle of polar coordinates; ρ is the distance from the origin of polar coordinates to the straight line; A, B, C, D are coefficients; x d and d are the coordinates of the tracked point.

[0117] In the above embodiment, each edge point has an interaction matrix and a projection line, all The interaction matrix that constitutes the system.

[0118] In a specific embodiment, the difference is made by the following formula:

[0119] e(t)=s(t)-s * (11)

[0120] Where: e(t) is the posture error; s(t) is the 6D posture of the bolt; s * is the expected posture.

[0121] S6: Calculate the movement speed of the depth camera according to the posture error of step S5.

[0122] In a specific embodiment, the movement speed of the depth camera is v c =(v x ,v y ,v z ,ω x ,ω y ,ω z );where v c =(v x ,v y,v z ) is the linear velocity of the depth camera, ω c =(ω x ,ω y ,ω z ) is the angular velocity of the depth camera; the movement speed of the depth camera is obtained by the following formula:

[0123]

[0124] Where: vc is the speed of the depth camera; λ is a constant (usually 1.5); is the expected position information of the target in the camera coordinate system; c t o is the position information of the target in the camera coordinate system; θu is the posture information of the target.

[0125] S7: Obtain the movement speed of the robotic arm through inverse kinematics according to the movement speed of the depth camera, and send the movement speed of the robotic arm to the robotic arm to make the robotic arm move.

[0126] In a specific embodiment, the linear speed of the robot during visual servoing is as follows: Figure 8 shown.

[0127] S8: Repeat steps S5-S8 until the posture error of step S5 is less than the posture error threshold. At this time, the visual servoing is completed, the robot arm stops moving, and the posture of the depth camera is parallel to the bolt.

[0128] S9: Use the move L command to align the bolts of the end effector mounted at the end of the robot arm.

[0129] The above description is only a preferred embodiment of the present invention and does not constitute any form of limitation to the present invention. Although the present invention has been disclosed as a preferred embodiment as above, it is not intended to limit the present invention. Any technician familiar with the profession can make some changes or modifications to equivalent embodiments of equivalent changes using the technical contents disclosed above without departing from the scope of the technical solution of the present invention. However, any simple modification, equivalent change and modification made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention still falls within the scope of the technical solution of the present invention.

Claims

1. A 6D pose detection and visual servoing method for bolts based on 3D template matching, characterized in that: The following steps are involved: S1: Acquire an actual image of a target bolt, and establish a CAD model of the bolt according to the actual image; S2: photographing a target bolt using a depth camera installed at the end of the robotic arm to obtain a bolt image, and performing image preprocessing on the bolt image; S3: Detect bolt feature information according to the bolt image after image preprocessing to obtain feature points, feature lines and feature surfaces; S4: matching the initial point of the CAD model in step S1 with the initial point of the bolt obtained by detecting the bolt feature information in step S3, and storing the matched feature information; S5: Tracking and estimating the 6D pose of the bolt, and subtracting the estimated 6D pose from the set desired pose, and using the subtraction result as the input of the visual servo algorithm; S6: Calculate the movement speed of the depth camera according to the posture error of step S5; S7: obtaining the movement speed of the robotic arm through inverse kinematics according to the movement speed of the depth camera, and sending the movement speed of the robotic arm to the robotic arm to make the robotic arm move; S8: repeating steps S5-S8 until the posture error of step S5 is less than the posture error threshold, at which time the visual servoing is completed, the robot arm stops moving, and the posture of the depth camera is parallel to the bolt; S9: Use the move L command to align the bolts of the end effector mounted at the end of the robot arm.

2. The 6D position detection and visual servoing method of bolts based on 3D template matching according to claim 1 is characterized in that: In step S1, after the CAD model of the bolt is established, it is saved in CAD format.

3. The 6D position detection and visual servoing method of bolts based on 3D template matching according to claim 1 is characterized in that: In step S2, the image preprocessing includes image segmentation processing and image grayscale processing.

4. The 6D posture detection and visual servoing method of bolts based on 3D template matching according to claim 3 is characterized in that: Gamma transformation is used to process image grayscale.

5. The 6D position detection and visual servoing method of bolts based on 3D template matching according to claim 1 is characterized in that: In step S5, when the bolt is tracked and the 6D pose is estimated, the information of the characteristic points and characteristic lines of the bolt is tracked, and the bolt is tracked and the pose is estimated by using a virtual visual servoing method.

6. The 6D position detection and visual servoing method of bolts based on 3D template matching according to claim 5 is characterized in that: The virtual visual servoing method is: Visual servo controls a virtual camera and a real camera, wherein the virtual camera and the real camera respectively obtain images of a target object; The edge method is used to track the edge line. During tracking, the visual feature p is composed of a set of distances d ⊥ Composition, the local point feature p obtained by the tracker t The distance from the contour of the object L, at which point the expected eigenvalue p*=0; Assuming that the contour of the target object can be described by straight lines or smooth curves, all distances are processed according to their corresponding line segments or ellipses; The alignment process of the projection features of the virtual camera and the real features of the real camera is the overlapping process of the virtual camera and the real camera, which is expressed by minimizing the error function: Where: Δ is the minimized error; n is the number of local feature points; p t i is the current local feature point; According to the current pose parameter μ t projection of the calculated three-dimensional model into the image; By minimizing the observed data p* and calculating the position of the same feature according to the current pose through forward projection The current bolt position is estimated by minimizing the error between: Where: pr ξ (μ, ω P i ) is the projection model determined according to the camera intrinsic parameter ξ and camera pose μ; p *i To minimize the observed data; At convergence, the pose that minimizes the error is assumed to be the true pose; The interaction matrix between the distance change between the fixed point and the moving line and the virtual camera movement is: l ρ =(Arcosθ+Brsinθ+C) / D (7) l θ =(Asinθ-Bcosθ) / D (8) α=x d sinθ-y d cosθ (9) Where: J d is the interaction matrix; are the interaction matrices of different edge points respectively; · represents transpose; N is the number of edge points of the object; is the straight line d l The Jacobian matrix of ; λ ρ , θ , α are intermediate parameters; θ is the angle of polar coordinates; ρ is the distance from the origin of polar coordinates to the straight line; A, B, C, D are coefficients; x d and d are the coordinates of the tracked point.

7. The 6D position detection and visual servoing method of bolts based on 3D template matching according to claim 1 is characterized in that: In step S6, the movement speed of the depth camera is obtained by the following formula: Where: v c is the speed of the depth camera; λ is a constant; c* t o is the expected position information of the target in the camera coordinate system; c t o is the position information of the target in the camera coordinate system; θu is the posture information of the target.

Images