Curved surface workpiece wiping robot sharing control method and system based on teaching key point normal weighting

Abstract

The invention belongs to the technical field related to robot sharing control, and discloses a method and a system for sharing control of a curved surface workpiece wiping robot based on teaching key point normal weighting. The method comprises the following steps that the tail end of the wiping robot is dragged to move to form a teaching track, and key points on the teaching track and normal vectors of the key points are determined; the expected posture of the tail end of the robot on the wiping track is calculated through the key points on the teaching track and the normal vectors of the key points; the man-machine shared control torque model and the robot dynamics model are solved, so that the actual posture of the tail end of the robot on the wiping track coincides with the expected posture, the tail end of the robot makes constant-force contact with a workpiece under autonomous control of the posture of the robot, and the position and force control torque and the posture control torque are obtained; and the robot is controlled according to the position, the force control torque and the attitude control torque, so that man-machine sharing control is realized. According to the invention, the problem that pose force coordination control depends on workpieces and system models is solved.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot shared control, and more specifically, relates to a method and system for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points. Background Art

[0002] In the human-robot collaborative wiping task of a curved workpiece, the operator not only needs to ensure that the end effector remains in contact with the workpiece surface, but also needs to adjust the robot's posture to adapt to the workpiece surface geometry and ensure constant force contact. However, this process increases the operator's task burden and reduces the continuity of the task trajectory. Therefore, it is necessary to propose a robot shared control method so that in the curved workpiece wiping task, the operator can arbitrarily drag the robot and plan the task trajectory based on his or her own experience. The robot can autonomously control the end effector posture to align it with the surface normal, while ensuring constant force contact between the robot and the curved workpiece. Therefore, it is necessary to establish a workpiece normal estimation model and a posture-force coordinated control framework.

[0003] Existing normal estimation models are mainly divided into offline and online methods. The offline method mainly involves point cloud scanning and environment reconstruction, and its normal estimation has high accuracy. However, due to the excessive density of point cloud data, its computational efficiency is relatively low. The online method mainly includes online measurement and estimation by laser and force sensors. Its normal estimation has good real-time performance, but its accuracy is easily affected by dynamic environmental disturbances. At the same time, in the robot developer wiping task, the robot end effector needs to be in continuous contact with the workpiece and maintain relative motion. At this time, the online estimation result of the force sensor will also be affected by unknown and difficult-to-model friction. Therefore, a workpiece normal estimation model based on the weighted normal of the teaching key points is proposed to improve the accuracy and efficiency of normal modeling of curved workpieces and improve the quality of curved workpiece wiping tasks.

[0004] Furthermore, within the pose-force coordinated control framework, model-based computational torque control linearizes the nonlinear robot system, reducing the difficulty of pose control and improving the ability to regulate the system's operating speed. However, pose tracking performance, especially posture tracking performance, requires high accuracy of the system model. Therefore, it is necessary to investigate methods that reduce the control's reliance on the system model while ensuring pose tracking accuracy. Furthermore, a unified torque control framework is designed by combining pose control and contact constant force control to achieve pose-force coordinated control of the system. Summary of the Invention

[0005] In response to the above defects or improvement needs of the prior art, the present invention provides a method and system for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points, which solves the problem that posture force coordinated control depends on the workpiece and system models.

[0006] To achieve the above object, according to one aspect of the present invention, a method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points is provided, the method comprising the following steps:

[0007] Drag the wiping robot end to form a teaching trajectory, and determine the key points on the teaching trajectory and the normal vectors of each key point;

[0008] The expected posture of the robot end on the wiping trajectory is calculated using the key points and normal vectors of the key points on the teaching trajectory;

[0009] Solve the human-machine shared control torque model and the robot dynamics model to obtain the position and force control torque and the posture control torque, so that the actual posture of the robot end on the wiping trajectory coincides with the expected posture and the robot end is in constant force contact with the workpiece under the autonomous control of the robot posture; control the robot according to the position and force control torque and the posture control torque to achieve human-machine shared control.

[0010] Further preferably, the human-machine shared control torque model and the robot dynamics model are as follows:

[0011]

[0012] Where F is the Cartesian space control torque, F e is the Cartesian space environmental moment measured by the sensor, F p is the position and force control torque, F o is the attitude control torque, ξ is the Cartesian space posture, is the velocity in Cartesian space, is the Cartesian space acceleration, M x (ξ), G x (ξ) are the Cartesian space inertia matrix, Coriolis force and centrifugal force matrices and gravity vector respectively.

[0013] Further preferably, the position and force control torque are as follows:

[0014]

[0015] Among them, F p is the position and force control torque, M x is the Cartesian space inertia matrix, Δp is the Cartesian space position error of the robot, is the velocity error in Cartesian space, is the expected acceleration in Cartesian space, K p is the positive definite stiffness matrix for position and force control, D p is the positive definite damping matrix for position and force control, ΔF is the force error, S pSelect the matrix for the constant force direction, 0 3×1 is the zero vector.

[0016] Further preferably, the posture control torque is as follows:

[0017]

[0018] Among them, F o is the attitude control torque, 0 3×1 is the zero vector, ΔR is the Cartesian space attitude error, ω c is the angular velocity in Cartesian space, K o is the attitude control positive definite stiffness matrix, D o is the positive definite damping matrix for attitude control.

[0019] Further preferably, the process of obtaining the normal vector of the key point is as follows:

[0020] Get the position of the key point and design the radius R with the key point as the center i The virtual sphere is calculated by calculating the two intersection points p between the teaching trajectory and the virtual sphere. i,1 and p i,2 ;

[0021] The normal vector of the key point is calculated according to the following relationship:

[0022]

[0023] in, is the normal vector of the i-th key point, p i is the position information of the i-th key point, p i,1 and p i,2 are the two intersection points of the teaching trajectory and the virtual ball, v i,approach is the key point p i The approach vector at v i,departure is the key point p i The away vector at .

[0024] Further preferably, the process of obtaining the expected normal vector is as follows:

[0025] Get a preset number of key points attached to the path point as the local key points of the path point;

[0026] The normal vector of the path point is obtained by weighted calculation using the normal vector of the local key point of the path point;

[0027] The normal vector of the path point is used to calculate the desired posture of the robot end.

[0028] Further preferably, the calculation formula of the normal vector of the path point is as follows:

[0029]

[0030] in, is the path point normal vector, is the matrix composed of the normal vectors of local key points, and W is the weight vector.

[0031] Further preferably, the calculation formula of the weight vector W is as follows:

[0032]

[0033] Among them, W is the weight vector, ω i is the Gaussian weight, k is the number of local key points, p c is the position information of the end effector, p i is the key point position information, and σ is the radius of the Gaussian function.

[0034] Further preferably, the desired posture is as follows:

[0035] R d =R(l,θ)R c

[0036]

[0037] Among them, R d is the desired rotation matrix, R c is the robot end rotation matrix, R(l,θ) is the rotation transformation matrix, l is the rotation axis, θ is the rotation angle, l1, l2, l3 are the three elements of the rotation axis, cosθ is the cosine of the rotation angle, and sinθ is the sine of the rotation angle.

[0038] According to another aspect of the present invention, a system for shared control of a curved workpiece wiping robot based on normal weighting of taught key points is provided, the system comprising an actuator for executing the above-mentioned method for shared control of a curved workpiece wiping robot based on normal weighting of taught key points.

[0039] In general, the above technical solutions conceived by the present invention have the following beneficial effects compared with the prior art:

[0040] 1. The present invention establishes a shared control method that integrates robot model and state feedback. The position is controlled by calculated torque, and the posture is controlled by feedback control torque. The contact constant force impedance control is integrated into the position calculated torque control framework to achieve coordinated control of the robot position, posture and contact force. When obtaining the desired posture, the path point normal vector is calculated according to the normal vector of the key point on the workpiece surface, and finally the desired posture is calculated, which does not rely on the workpiece model. In posture control, only the robot control level is involved, and it does not rely on the robot system model. In this way, the dependence on the workpiece and system model is reduced while ensuring the accuracy of system posture tracking.

[0041] 2. The present invention proposes a method for estimating the normal of curved workpieces. Combining the advantages of offline normal estimation accuracy and online normal calculation real-time, the density clustering algorithm is used to determine the key points of the teaching trajectory according to the teaching data and the vector cross multiplication method is used to calculate the normal of the key points. In the surface wiping task, the K nearest neighbor algorithm is used to determine the local key points around the path point and the normal of the path point is estimated in real time through Gaussian weighting of the key point normal, so as to achieve fast and accurate workpiece surface normal estimation and robot desired posture generation. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 The present invention is a schematic diagram of a method for sharing control of a developer wiping robot for a curved workpiece based on normal weighting of teaching key points, constructed according to a preferred embodiment of the present invention.

[0043] Figure 2 This is a diagram of the operator teaching trajectory and key point normal estimation model constructed according to the preferred embodiment of the present invention, where (a) is the projection of the operator teaching trajectory in the z direction of the robot, and (b) is the key point normal estimation model.

[0044] Figure 3 It is a workpiece normal estimation model diagram based on the normal weighting of the teaching key points constructed in accordance with the preferred embodiment of the present invention, wherein (a) is the task path point normal weighting model, and (b) is the robot expected posture generation model.

[0045] Figure 4 The diagram is a shared control framework diagram of Cartesian space robot model state fusion constructed according to a preferred embodiment of the present invention.

[0046] Figure 5 The present invention is based on a preferred embodiment of a robot developer wiping experimental platform, wherein (a) is the experimental setup, and (b) is a partial diagram of the curved surface wiping trajectory.

[0047] Figure 6 This is a task normal estimation and tracking result diagram obtained according to the preferred embodiment of the present invention.

[0048] Figure 7This is a diagram of the task constant force control result obtained according to the preferred embodiment of the present invention. DETAILED DESCRIPTION

[0049] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely for the purpose of explaining the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

[0050] A shared control method for a developer wiping robot for a curved workpiece based on normal weighting of a teaching key point is characterized in that the method comprises the following steps:

[0051] S1 collects the position and speed data of the operator during the drag teaching process, determines the key points of the teaching trajectory and calculates their normal vectors. Specifically, as follows:

[0052] The operator drags the robot to follow the broken line teaching trajectory, such as Figure 2 As shown in (a) above, the robot end-effector maintains continuous contact with the workpiece and remains at the turning point of the broken line trajectory for a relatively long time to obtain more accurate turning point position information. The robot end-effector position and velocity data are collected at a fixed frequency throughout the drag teaching process to determine the teaching key point position and calculate its normal vector.

[0053] Since the data points at the turning points of the broken line trajectory are more densely distributed, the density-based clustering algorithm DBSCAN is used to determine the turning point position of the broken line trajectory, that is, the teaching key point, according to the position data and speed data during the drag teaching process. Where i = 1, 2,…, K, K is the number of key points.

[0054] Get the key point position information p i Then, the radius R is designed with the key point as the center i A virtual ball, such as Figure 2 In one embodiment of the present invention, the radius of the virtual sphere is designed to be 0.01 m. Calculate the two intersection points p of the teaching trajectory and the virtual sphere i,1 and p i,2 ;

[0055] Constructing proximity vectors and away from the vector

[0056] At this time, the key point normal vector can be obtained by vector cross product Where S(v i,departure ) is an antisymmetric matrix, is the normal vector of the key point. It is worth noting that the calculated normal directions of the key points are inconsistent. A simple judgment statement is used to set the normal direction of the key points to face outward from the workpiece.

[0057] S2 obtains local key points according to the position of the task path points, estimates the normal of the path points using the key point normal weighting method, and calculates the expected posture of the robot for robot posture control.

[0058] For the contact point between the end effector and the workpiece in the wiping task, the workpiece normal vector at the contact point is calculated by weighting the normal vectors of its local key points. Figure 3 As shown in (a). Define the contact point position of the end effector in the wiping task The K nearest neighbor algorithm is used to search for the k local key points and their normal vectors closest to the contact point.

[0059] The contact point normal vector is calculated using the local weighted Gaussian function method.

[0060]

[0061] in, is the path point normal vector, is the matrix composed of the normal vectors of local key points, and W is the weight vector. W is the weight vector, ω i is the Gaussian weight, k is the number of local key points, p c is the position information of the end effector, p i is the key point position information, and σ is the radius of the Gaussian function.

[0062] The desired posture information of the robot is calculated based on the normal information of the workpiece at the contact point, which is used for autonomous posture control in the shared control framework of subsequent robot model and state fusion, such as Figure 3 As shown in (b). Define the end effector contact point rotation matrix as R c =[n c ,o c ,a c ], the desired posture of the end effector is R d =[n d ,o d ,a d ], in order to align the z-axis direction of the end effector contact point with the workpiece normal, it is necessary to rotate the end effector contact point clockwise around the fixed axis l by an angle θ, where At this time, the expected posture of the end effector contact point is R d =R(l,θ)R c ,in

[0063]

[0064] S3 designs a shared control method that integrates robot model and state to achieve coordinated control of the robot's end effector position, posture and contact force, such as Figure 4 shown.

[0065] According to the robot joint space dynamics model, its Cartesian space dynamics model is established.

[0066] The joint space dynamics model of the n-DOF rotational joint robot is as follows:

[0067]

[0068] in, are the robot's joint angular position, joint angular velocity, and joint angular acceleration, respectively. are the joint space inertia matrix, the Coriolis force and centrifugal force matrix, and the gravity vector, respectively. J(q) is the Jacobian matrix, τ is the control torque, and τ e is the environmental torque. M(q), C(q), G(q), and J(q) can be obtained through robot dynamic parameter identification or directly from the robot API interface.

[0069] According to the relationship between the robot joint space velocity and the Cartesian space velocity Relationship between joint space torque and Cartesian space torque The Cartesian space dynamics model of the n-DOF rotary joint robot is as follows:

[0070]

[0071] in, is the robot's Cartesian space velocity, is the linear velocity in Cartesian space, is the angular velocity in Cartesian space. are the Cartesian space inertia matrix, the Coriolis force and centrifugal force matrix, and the gravity vector, respectively. F is the Cartesian space control torque, and F e is the Cartesian space environment torque measured by the sensor. To simplify the expression, it is expressed as M x , C x , G x Each matrix is ​​represented in the form of .

[0072] A shared control framework integrating robot model and state is designed to realize operator position drag control, robot posture autonomous control and robot contact constant force control.

[0073] Design the shared control torque of the robot model and state fusion as follows:

[0074]

[0075] By design variable F p and F o The robot's Cartesian space position and posture are controlled. Substituting the shared control torque into the robot's dynamic equation, we can get

[0076] Decomposition can be obtained It can be further decomposed into robot position control and robot posture control

[0077] For robot position control, the calculation torque control method is used to improve the position and speed control ability of the robot during movement. The position control torque in the robot shared control framework is designed as follows:

[0078]

[0079] Where Δp=p d -p c is the Cartesian space position error of the robot, p c is the Cartesian space position, p d is the expected position in Cartesian space, is the velocity error in Cartesian space, is the expected acceleration in Cartesian space, is a positive definite control matrix.

[0080] At the same time, the constant force impedance control is integrated into the computational torque control framework to achieve the unification of position control and force control. Therefore, on the basis of position control, the force error term is added to achieve position control in the motion direction and constant force control in the contact direction. The specific form is as follows:

[0081]

[0082] Where ΔF = F ep -F d is the force error, F ep F e The first three elements of F d is the expected contact force, S p =diag(0,0,1) is the constant force direction selection matrix. The constant force control is realized in the z direction of the robot. The control parameter of this direction is k z =0,b z =b+Δb(t).

[0083]

[0084] at this time is a small value to prevent the denominator from being 0. Is a custom update rate.

[0085] For robot posture control, a feedback control method is used to reduce the dependence of posture control on the robot model. The posture control torque in the robot shared control framework is designed as follows:

[0086]

[0087] where ω c is the robot's angular velocity in Cartesian space, is the Cartesian space pose error of the robot. The Cartesian space pose error is calculated by the end effector contact point rotation matrix and the desired pose. is a positive definite control matrix.

[0088] The S4 operator drags the robot to perform the surface wiping task, estimates the workpiece normal in real time based on the position of the robot's contact point, and combines the proposed shared control method to achieve autonomous control of the robot's posture and constant contact force control.

[0089] Set appropriate position and constant force control parameters K according to control performance requirements p , D p , b, ε and σ, attitude control parameters K o , D o , as well as the robot control frequency and force sensor sampling frequency, to perform the developer wiping task of the curved workpiece.

[0090] In one embodiment of the present invention, a curved workpiece robot developer wiping experimental platform is constructed, such as Figure 5 As shown in (a), it includes a robot, a force sensor, a wiping tool, and a curved workpiece. The specific experimental process is as follows:

[0091] Set the position control coefficient according to the control performance requirements

[0092]

[0093] where k z =0,b z The designed constant force control rate is in line with the designed constant force control rate. In the designed constant force control rate, b=10,σ=0.005,ε=10 -8 . At the same time, set the attitude control coefficient and The robot control frequency is 200 Hz and the force sensor sampling frequency is 200 Hz.

[0094] The operator drags the robot so that its end effector is in constant contact with the environment and moves along Figure 5The trajectory shown in (b) is used to perform the curved workpiece wiping task. During the task, the workpiece normal estimation model based on the weighted normal of the key points taught is combined with the contact position information between the robot end effector and the workpiece to calculate the expected posture of the robot end contact point in real time. Finally, the rotation matrix and the 5N expected contact force information are input into the shared control framework, so that the normal of the robot end effector and the workpiece normal can be autonomously aligned and controlled, and the contact constant force control can be achieved at the same time. The normal tracking result is shown in Figure 1. Figure 6 As shown, the constant force tracking situation is as follows Figure 7 As shown. Finally, the entire curved workpiece wiping task is completed.

[0095] It will be easily understood by those skilled in the art that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points, characterized in that: The method comprises the following steps: Drag the wiping robot end to form a teaching trajectory, and determine the key points on the teaching trajectory and the normal vectors of each key point; The expected posture of the robot end on the wiping trajectory is calculated using the key points and normal vectors of the key points on the teaching trajectory; Solve the human-machine shared control torque model and the robot dynamics model to obtain the position and force control torque and the posture control torque, so that the actual posture of the robot end on the wiping trajectory coincides with the expected posture and the robot end is in constant force contact with the workpiece under the autonomous control of the robot posture; control the robot according to the position and force control torque and the posture control torque to achieve human-machine shared control.

2. The method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 1, characterized in that: The human-machine shared control torque model and robot dynamics model are as follows: Where F is the Cartesian space control torque, F e is the Cartesian space environmental moment measured by the sensor, F p is the position and force control torque, F o is the attitude control torque, ξ is the Cartesian space posture, is the velocity in Cartesian space, is the Cartesian space acceleration, M x (ξ), G x (ξ) are the Cartesian space inertia matrix, the Coriolis force and centrifugal force matrices, and the gravity vector respectively.

3. The method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 1, characterized in that: The position and force control torque are as follows: Among them, F p is the position and force control torque, M x is the Cartesian space inertia matrix, Δp is the Cartesian space position error of the robot, is the velocity error in Cartesian space, is the expected acceleration in Cartesian space, K p is the positive definite stiffness matrix for position and force control, D p is the positive definite damping matrix for position and force control, ΔF is the force error, S p Select the matrix for the constant force direction, 0 3×1 is the zero vector.

4. The method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 3, characterized in that: The attitude control torque is as follows: Among them, F o is the attitude control torque, 0 3×1 is the zero vector, ΔR is the Cartesian space attitude error, ω c is the angular velocity in Cartesian space, K o is the attitude control positive definite stiffness matrix, D o is the positive definite damping matrix for attitude control.

5. The method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 1, characterized in that: The process of obtaining the normal vector of the key point is as follows: Get the position of the key point, and calculate the two intersection points p of the virtual sphere with the key point as the center. i,1 and p i,2 ; The normal vector of the key point is calculated according to the following relationship: in, is the normal vector of the i-th key point, p i is the position information of the i-th key point, p i,1 and p i,2 are the two intersection points of the teaching trajectory and the virtual ball, v i,approach is the key point p i The approach vector at v i,departure is the key point p i The away vector at .

6. The method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 5, characterized in that: The process of obtaining the desired posture of the robot end at the path point is as follows: Get a preset number of key points around the path point as the local key points of the path point; The normal vector of the path point is obtained by weighted calculation using the normal vector of the local key point of the path point; The normal vector of the path point is used to calculate the desired posture of the robot end.

7. The method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 6, characterized in that: The calculation formula of the normal vector of the path point is as follows: in, is the path point normal vector, is the matrix composed of the normal vectors of local key points, and W is the weight vector.

8. The method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 7, characterized in that: The calculation formula of the weight vector W is as follows: W=[ω1,ω2,…,ω i ,…,oh k ] T Among them, W is the weight vector, ω i is the Gaussian weight, k is the number of local key points, p c is the position information of the end effector, p i is the key point position information, and σ is the radius of the Gaussian function.

9. A method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points according to claim 1 or 6, characterized in that: The desired posture is as follows: R d =R(l,θ)R c Among them, R d is the desired rotation matrix, R c is the robot end rotation matrix, R(l,θ) is the rotation transformation matrix, l is the rotation axis, θ is the rotation angle, l1, l2, l3 are the three elements of the rotation axis, cosθ is the cosine of the rotation angle, and sinθ is the sine of the rotation angle.

10. A system for shared control of curved workpiece wiping robots based on normal weighting of key points taught, characterized in that: The system includes an actuator, which is used to execute the method for shared control of a curved workpiece wiping robot based on normal weighting of teaching key points as described in any one of claims 1 to 9.

Images