A dynamic assembly method for an insulator replacement robot with pose decoupling

Abstract

The present invention relates to a dynamic assembly method for an insulator replacement robot with pose decoupling. The method includes: Step 1: Collect the point cloud image of the insulator surface through a depth camera, locate the plane of the pin hole, and extract its normal vector as the direction of the pin hole axis; Step 2: Determine the target pose of the end of the robotic arm according to the direction of the pin hole axis and the right-hand rule; control the movement of the robotic arm to adjust the target pose of the pin pushing tool; Step 3: Adjust the position through a compliant control algorithm so that the pin pushing tool pushes the positioning pin out of the pin hole to complete the dynamic assembly with pose decoupling.

Description

Technical Field

[0001] The present invention relates to the field of compliant assembly, and more specifically, to a dynamic assembly method for an insulator replacement robot with pose decoupling. Background Art

[0002] The development of robot control technology enables robots to complete various precision assembly tasks. In these assembly tasks, the relative position between the operation platform and the robot base is fixed. Based on the compliant assembly method, the robot makes different actions according to different values of the contact force. However, when the operation platform is in an environment with cable-like characteristics, a tight dynamic rigid-flexible coupling relationship will be formed between the robot and the operation platform. The position of the cable will undergo complex dynamic changes due to the extrusion of the robot, and the dynamic changes in the position will in turn cause the contact force to have instantaneous dynamic noise, which will greatly affect the compliance of the assembly action. Therefore, such assembly tasks where the assembly platform is not fixed and the position is affected by the movement of the robot are defined as dynamic assembly tasks, and the cable assembly environment is a typical application scenario. How to ensure the compliance of the assembly action in a dynamic rigid-flexible coupling environment is a key scientific issue in dynamic assembly tasks.

[0003] In the insulator replacement task, it is necessary to first push out the insulation pin that plays a fixing role between two insulators. Before that, it is necessary to assemble the pin-pushing tool into the pin hole. Since the insulator is located on a transmission line with cable-like characteristics, this assembly task can be considered a dynamic assembly task. After simplifying the pin-pushing tool into a shaft model and the pin hole into a hole model, the control method of shaft-hole assembly can be used for analysis. In general shaft-hole assembly tasks, usually, shaft-hole assembly methods based on physical modeling, compliant assembly methods based on passive compliant control and active compliant control, and force / position hybrid control assembly methods can be adopted. However, in these assembly tasks, the assembly environment model generally remains unchanged. Summary of the Invention

[0004] The present invention solves the problem that traditional control methods cannot complete dynamic assembly tasks. By using a pose decoupling assembly method, with the participation of depth vision, the normal vector of the point cloud is extracted, and then the attitude adjustment is realized. After that, the position is separately controlled compliantly.

[0005] A dynamic assembly method for an insulator replacement robot with pose decoupling, used for performing pose decoupling dynamic assembly of a pin-pushing tool and a pin hole when replacing an insulator on a transmission line with cable-like characteristics, includes the following steps:

[0006] Step 1: Collect the point cloud image of the insulator surface through a depth camera, locate the pin hole plane, and extract its normal vector as the pin hole axis direction;

[0007] Step 2: Determine the target pose of the end of the robotic arm according to the axis direction of the pin hole and the right-hand rule; control the movement of the robotic arm to adjust the target pose of the pushing tool.

[0008] Step 3: Adjust the position through the compliance control algorithm so that the pushing tool pushes out the positioning pin from the pin hole, completing the decoupled dynamic assembly of the pose.

[0009] The point cloud image of the insulator surface collected by the depth camera includes the positioning pin hole.

[0010] The plane of the positioning pin hole includes: obtaining the point cloud of the flat area on the insulator surface on one side of the pin hole using the depth camera, and regarding the normal vector of the point cloud in this area as the axis direction of the pin hole.

[0011] The PCA algorithm is used to extract the normal vector of the insulator surface point cloud.

[0012] Using the PCA method to estimate the normal vector of the point cloud includes:

[0013] a. Define the center point of the point set as m, then:

[0014]

[0015] where the pin hole point cloud set X = {x i};

[0016] b. Through the least squares method, make the sum of the squares of the dot products of the vectors from all points in the point set to the center point and the normal vector n the smallest, and solve for n through an optimization method. The optimization objective is:

[0017]

[0018] Define x i -m as y i , and the above formula can be rewritten as:

[0019]

[0020] Let

[0021] y i y i T = s i

[0022] Get:

[0023]

[0024] After substituting the element summation matrix in the above formula, we can get:

[0025]

[0026] Where S is the covariance matrix of the point set, and its representation is as follows:

[0027]

[0028] Using the Lagrange multiplier method to solve the constrained problem, we have:

[0029] L(n,λ)=n T Sn-λ(n T n-1)

[0030] Respectively, take the partial derivatives of L with respect to n and λ and set them equal to 0, then we have:

[0031]

[0032]

[0033] The final n obtained is the eigenvector of the S matrix corresponding to the eigenvalue λ;

[0034] Perform singular value decomposition (SVD) on the S matrix, that is:

[0035] S=U∑V T

[0036] Calculate the eigenvector corresponding to the minimum eigenvalue, which is the normal vector n.

[0037] The method for determining the target pose of the end of the robotic arm according to the axis direction of the pin hole and the right-hand rule includes:

[0038] a. Normalize the extracted axis direction of the hole and define it as the z-axis of the target pose;

[0039] b. Find an arbitrary unit vector perpendicular to the z-axis and define it as the y-axis of the target pose;

[0040] c. Obtain the x-axis of the target pose according to the right-hand rule.

[0041] The method for controlling the robotic arm to move and adjust the target pose of the pushing tool includes:

[0042] Calculate the desired rotation matrix between the current pose and the target pose of the end link coordinate system of the robotic arm;

[0043] Use the pose control algorithm to achieve pose adjustment according to the desired rotation matrix; the pose control algorithm includes angle control and speed control;

[0044] When the end of the robotic arm reaches the target pose, the pushing tool at its end is just aligned with the pin hole, and the axis of the pushing tool is consistent with the axis of the pin hole.

[0045] The adjustment of the position is realized by using a compliant control algorithm to control the feeding position of the pushing tool towards the pin hole direction.

[0046] The pushing tool is a push rod installed at the end of the robotic arm.

[0047] A dynamic assembly system for an insulator replacement robot with pose decoupling includes: a depth camera, a controller, and a robotic arm;

[0048] The depth camera is used to collect images of the insulator pin holes and send them to the controller;

[0049] The controller is used to calculate the axis direction of the pin hole according to the pin hole position, obtain the target pose and position of the end of the robotic arm, control the robotic arm to drive the pushing tool at the end to move, and push out the positioning pin from the pin hole;

[0050] The robotic arm has a pushing tool installed at its end for pushing out the positioning pin.

[0051] The beneficial effects and advantages of the present invention are as follows:

[0052] Through pose decoupling control, the method of the present invention can control the camera of the robotic arm module to collect pin hole images. The controller determines the target pose of the end of the robotic arm according to the axis position of the pin hole, thereby controlling the end of the robotic arm to drive the pushing tool to move to the target pose. Then, a compliant control algorithm is used to adjust the feeding position, insert the pushing tool into the pin hole on the insulator, and push out the positioning pin. The method of the present invention improves the success rate of assembly in a dynamic environment and makes the control process more transparent. Description of the Drawings

[0053] Figure 1 Surface view of the insulator in hole assembly.

[0054] Figure 2 Block diagram of the dynamic assembly method with pose decoupling. Detailed Embodiments

[0055] To make the above objects, features, and advantages of the present invention more obvious and understandable, the following detailed description of the specific implementation method of the present invention will be given in conjunction with the drawings. Many specific details are set forth in the following description to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without departing from the connotation of the invention. Therefore, the present invention is not limited by the specific implementations disclosed below.

[0056] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field to which the present invention belongs. The terms used in the description of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0057] As Figure 1 shown, it is a surface diagram of the insulator in hole assembly.

[0058] The hardware devices required for the present invention are: a robotic arm, a controller, and a depth camera. The depth camera is used to capture the image of the pin hole on the surface of the insulator and send it to the controller. A pin pushing device (push rod) is connected to the end of the robotic arm. The pin pushing device is used to push out the pin in the pin hole. The controller identifies and locates the posture of the pin hole based on the machine vision of the depth camera and sets it as the target posture, and obtains the desired rotation matrix from the target posture and the posture of the end coordinate system in the base coordinate system of the robotic arm. Then, using the posture control algorithm, the axis of the pin pushing tool is aligned with the axis of the pin hole. Finally, under the action of position compliance control, the pin pushing tool is inserted into the pin hole on the insulator to complete the assembly and push out the pin.

[0059] As Figure 2 shown, the present invention is a dynamic assembly method for an insulator replacement robot with pose decoupling, and its specific implementation steps are as follows:

[0060] Step 1: Collect the point cloud of the insulator surface including the pin hole through the depth camera, and extract its normal vector as the direction of the hole axis.

[0061] Step 2: Take the extracted hole axis direction as the z-axis of the target posture of the robotic arm, and confirm the directions of other coordinate axes in the target posture according to the right-hand rule, which is defined as the target posture. Finally, obtain the desired rotation matrix through the coordinate transformation between the target posture and the current end link coordinate system of the robotic arm, and then realize the posture control.

[0062] Step 3: Then adjust the position through the compliance control algorithm so that the pin pushing tool at the end of the robotic arm is inserted into the pin hole to push out the positioning pin and complete the assembly.

[0063] Step 1 is to use the depth camera to obtain the point cloud of the flat area on the insulator surface on one side of the pin hole. Then, since the plane of this area is nearly perpendicular to the direction of the pin hole axis, the normal vector of the point cloud here is regarded as the direction of the pin hole axis. The extraction of the normal vector of the point cloud is obtained through the PCA algorithm.

[0064] Estimate the normal vector of the point cloud using the PCA method. First, set the point set X = {x i}, and denote the center point of the point set as m, then there is:

[0065]

[0066] Since the normal vector of a plane is perpendicular to all vectors in the plane, based on the feature that the dot product of perpendicular vectors is equal to 0, we can find the normal vector of the plane represented by the point set and define it as n. Here, using the least squares approach, we minimize the sum of the squares of the dot products of the vectors from all points in the point set to the center point and the normal vector n, and solve for n through optimization. The optimization objective is:

[0067]

[0068] Define x i -m as y i , and the above equation can be rewritten as:

[0069]

[0070] Let

[0071] y i y i T = s i

[0072] We get:

[0073]

[0074] After replacing with the element summation matrix in the above equation, we can get:

[0075]

[0076] where S is the covariance matrix of the point set, and its representation is as follows:

[0077]

[0078] Using the Lagrange multiplier method to solve the constrained problem, we have:

[0079] L(n,λ) = n T Sn - λ(n T n - 1)

[0080] Respectively, take the partial derivatives of L with respect to n and λ and set them equal to 0, then we have:

[0081]

[0082]

[0083] The final n obtained is the eigenvector corresponding to the eigenvalue λ in the S matrix. Therefore, we perform singular value decomposition (SVD) on the S matrix, that is:

[0084] S = U∑V T

[0085] The column vectors in U are the eigenvectors corresponding to the eigenvalues at the diagonal positions, that is, the eigenvectors corresponding to the matrix S. Among them, the eigenvector corresponding to the largest eigenvalue is the main direction of the points in this point set, and the eigenvector corresponding to the smallest eigenvalue represents a direction that has nothing to do with the directions represented by all sample points. That is the normal vector we need to find.

[0086] Step 2 is to unitize the extracted hole axis direction and use it as the z-axis of the target pose of the robotic arm, and then determine the orientation of the target pose coordinate system. Then find any unit vector perpendicular to the z-axis and define it as the y-axis of the target pose. Finally, obtain the representation form of the x-axis of the target pose according to the right-hand rule. After obtaining the representation of the target pose, obtain the desired rotation matrix between the target pose and the current end-link coordinate system of the robotic arm through coordinate transformation, and then use the attitude control algorithm to achieve attitude adjustment. Attitude control algorithms include angle control, speed control, etc.

[0087] Step 3 is to use the position compliance control method to control the feed position in the advancing direction of the shaft body. Since the attitude of the pin hole changes little in the dynamic environment, the position and attitude decoupling control can be used, that is, first perform attitude control according to the point cloud processing result, so that the end of the robotic arm drives the pin tool to move to the front end of the pin hole, making the axis of the pin tool coincide with the axis of the pin hole. When the end attitude of the robotic arm is adjusted to the target attitude, then adjust the feed position separately to make the pin toolbox advance in the direction of the pin hole and push the locating pin out of the pin hole.

[0088] Compliance control algorithms can adopt existing algorithms such as impedance control, admittance control, force / position hybrid control, reinforcement learning control, etc.

[0089] The specific embodiments described above further elaborate on the purpose, technical solutions, and beneficial effects of the present invention. It should be understood that the above are only specific embodiments of the present invention and are not used to limit the present invention. Any modifications, equivalent replacements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A dynamic assembly method for an insulator replacement robot with pose decoupling, characterized in that, When replacing the insulator on a transmission line with the characteristics of a flexible cable, the decoupling dynamic assembly of the pushing tool and the pin hole is carried out, including the following steps: Step 1: Collect the point cloud image of the insulator surface through a depth camera, locate the pin hole plane, and extract its normal vector as the direction of the pin hole axis; Step 2: Determine the target posture of the end of the robotic arm according to the direction of the pin hole axis and the right-hand rule; control the movement of the robotic arm to adjust the target posture of the pushing tool; Step 3: Adjust the position through a compliance control algorithm so that the pushing tool pushes the positioning pin out of the pin hole to complete the decoupling dynamic assembly of the pose; The control of the robotic arm to move and adjust the target posture of the pushing tool includes: Calculate the expected rotation matrix between the current posture and the target posture of the end link coordinate system of the robotic arm; Use the attitude control algorithm to achieve attitude adjustment according to the expected rotation matrix; the attitude control algorithm includes angle control and speed control; When the end of the robotic arm reaches the target posture, the pushing tool at its end is just aligned with the pin hole, and the axis of the pushing tool is consistent with the axis of the pin hole; The adjustment of the position is to use a compliance control algorithm to achieve the feed position control of the pushing tool in the direction of the pin hole; The point cloud image of the insulator surface collected by the depth camera includes the positioning pin hole; The pin hole plane includes: using the depth camera to obtain the point cloud of the flat area on the insulator surface on one side of the pin hole, and taking the normal vector of this area of the point cloud as the direction of the pin hole axis; The extraction of the normal vector of the insulator surface point cloud is carried out by the PCA algorithm; Using the PCA method to estimate the normal vector of the point cloud includes: a. Define the center point of the point set as m, then there is: Among them, the pin hole point cloud set X = {x i}; b. Through the least squares method, make the sum of the squares of the dot products of the vectors from all points in the point set to the center point and the normal vector n the smallest, and solve n by an optimization method. The optimization objective is: Define x i -m as y i , then the above equation can be rewritten as: Let Get: After replacing with the element summation matrix in the above formula, we can get: Where S is the covariance matrix of the point set, and its representation is as follows: Use the Lagrange multiplier method to solve the constrained problem, then there is: L(n,λ)=n T Sn - λ(n T n - 1) Let the partial derivatives of L with respect to n and λ be taken respectively and set them equal to 0, then there is: The final n obtained is the eigenvector of the S matrix corresponding to the eigenvalue λ; Perform singular value decomposition (SVD) on the S matrix, that is: S = U∑V T Calculate the eigenvector corresponding to the smallest eigenvalue, which is the normal vector n.

2. The dynamic assembly method of an insulator replacement robot with pose decoupling according to claim 1, characterized in that The determination of the target posture of the end of the robotic arm according to the direction of the pin hole axis and the right-hand rule includes: a. Unitize the extracted hole axis direction and define it as the z-axis of the target posture; b. Find an arbitrary unit vector perpendicular to the z-axis and define it as the y-axis of the target posture; c. Obtain the x-axis of the target posture according to the right-hand rule.

3. A dynamic assembly method for an insulator replacement robot with pose decoupling according to any one of claims 1-2, characterized in that, The pushing tool is a push rod installed at the end of the robotic arm.

4. The assembly system of a pose decoupled insulator replacement robot dynamic assembly method according to claim 1, characterized in that, It includes: A depth camera, a controller, and a robotic arm; The depth camera is used to collect the insulator pin hole image and send it to the controller; The controller is used to calculate the pin hole axis direction according to the pin hole position, obtain the target posture and position of the end of the robotic arm, and control the robotic arm to drive the pushing tool at the end to move and push the positioning pin out of the pin hole; The robotic arm has a pushing tool installed at its end for pushing the positioning pin out.

Images