Method and system for shared control of a curved workpiece wiping robot based on teaching key point normal weighting

Abstract

The present application belongs to the technical field of robot shared control, and discloses a method and system for shared control of a wiping robot for a curved workpiece based on a teaching key point normal weighting method. The method comprises the following steps: dragging the end of the wiping robot to move to form a teaching trajectory, determining key points on the teaching trajectory and normal vectors of each key point; calculating the desired pose of the end of the robot on the wiping trajectory using the key points on the teaching trajectory and the normal vectors of the key points; solving a human-robot shared control torque model and a robot dynamics model so that the actual pose of the end of the robot on the wiping trajectory coincides with the desired pose, and the end of the robot is in constant force contact with the workpiece under autonomous control of the robot pose, obtaining a position and force control torque and a pose control torque; and controlling the robot according to the position and force control torque and the pose control torque, thereby achieving human-robot shared control. Through the present application, the problem of position and force coordinated control depending on the workpiece and the system model is solved.

Description

Technical Field

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

[0002] In collaborative human-robot cleaning tasks on curved workpieces, the operator not only needs to ensure the end effector remains in contact with the workpiece surface, but also needs to adjust the robot's posture to adapt to the workpiece's geometry and ensure constant force contact. However, this process increases the operator's workload and reduces the continuity of the task trajectory. Therefore, a robot-shared control method is needed, allowing the operator to arbitrarily drag the robot and plan the task trajectory based on their experience during the curved workpiece cleaning task. The robot can autonomously control the end effector's posture to align with the surface normal, while ensuring constant force contact between the robot and the curved workpiece. Therefore, a workpiece normal estimation model and a pose-force coordination control framework need to be established.

[0003] Existing normal estimation models are mainly divided into offline and online methods. Offline methods primarily involve point cloud scanning and environment reconstruction, offering high accuracy in normal estimation. However, the high density of point cloud data results in relatively low computational efficiency. Online methods mainly involve online measurement and estimation using lasers and force sensors, providing good real-time performance in normal estimation, but their accuracy is easily affected by dynamic environmental disturbances. Furthermore, in robotic developer wiping tasks, the robot's end effector needs to maintain continuous contact and relative motion with the workpiece, where the online force sensor estimation results are also affected by unknown and difficult-to-model frictional forces. Therefore, this paper proposes a workpiece normal estimation model based on weighted normals from taught key points to improve the accuracy and efficiency of normal modeling for curved workpieces and enhance the quality of curved workpiece wiping tasks.

[0004] Furthermore, for 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, its pose tracking performance, especially attitude tracking performance, requires high accuracy of the system model. Therefore, it is necessary to study methods to reduce the control's dependence on the system model while ensuring the accuracy of system pose tracking. Simultaneously, a unified torque control framework is designed by combining pose control and contact constant force control to achieve coordinated pose-force control of the system. Summary of the Invention

[0005] To address the aforementioned deficiencies or improvement needs of existing technologies, this invention provides a method and system for shared control of a surface workpiece wiping robot based on teaching key point normal weighting, solving the problem that pose and force coordination control depends on the workpiece and system model.

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

[0007] The end effector of the wiping robot is dragged to form a teaching trajectory, and the key points on the teaching trajectory and the normal vectors of each key point are determined.

[0008] The desired pose of the robot's end effector on the wiping trajectory is calculated using key points and their normal vectors on the teaching trajectory.

[0009] Solve the human-machine shared control torque model and robot dynamics model to obtain the position and force control torque and attitude control torque, so that the actual posture of the robot end on the wiping trajectory coincides with the desired posture and the robot end makes 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 attitude control torque to realize human-machine shared control.

[0010] More 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 F is the Cartesian space environment torque measured by the sensor. p For position and force control torque, F o Let ξ be the attitude control torque, and ξ be the pose in Cartesian space. For Cartesian space velocity, For Cartesian space acceleration, M x (ξ), G x (ξ) represents the Cartesian space inertial matrix, the Coriolis force and centrifugal force matrix, and the gravity vector, respectively.

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

[0014]

[0015] Among them, F p For position and force control torque, M x Let be the Cartesian inertia matrix, and Δp be the robot's Cartesian position error. For Cartesian space velocity error, For the expected acceleration in Cartesian space, K p D is the positive definite stiffness matrix controlled by position and force. p Here, S is the positive definite damping matrix for position and force control, ΔF is the force error, and S is the position and force control matrix. pChoose a matrix for the direction of constant force, 0 3×1 It is a zero vector.

[0016] More preferably, the attitude control torque is as follows:

[0017]

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

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

[0020] Obtain the location of the key points, and design a sphere with radius R centered on the key points. i For a virtual sphere, calculate the two intersection points p between the teaching trajectory and the virtual sphere. i,1 and p i,2 ;

[0021] Calculate the normal vector of the key point according to the following formula:

[0022]

[0023] in, Let p be the normal vector of the i-th key point. i It is the location information of the i-th key point, p i,1 and p i,2 v represents the two intersection points of the teaching trajectory and the virtual sphere. i,approach For key point p i The nearest vector at point v i,departure For key point p i The vector away from the point.

[0024] More preferably, the process of obtaining the desired normal vector is as follows:

[0025] Obtain a preset number of key points near the path point as local key points of that path point;

[0026] The normal vector of a path point is obtained by weighted calculation using the normal vectors of the local key points of the path point.

[0027] The desired pose of the robot's end effector is calculated using the normal vectors of the path points.

[0028] More preferably, the formula for calculating the normal vector of the path point is as follows:

[0029]

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

[0031] More preferably, the formula for calculating the weight vector W is as follows:

[0032]

[0033] Where W is the weight vector, ω i Here are the Gaussian weights, k is the number of local keypoints, and p... c For end effector position information, p i σ represents the location information of key points, and σ is the radius of the Gaussian function.

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

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

[0036]

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

[0038] According to another aspect of the present invention, a system for shared control of a surface workpiece wiping robot based on teach-key point normal weighting is provided. The system includes an actuator for performing the above-described method for shared control of a surface workpiece wiping robot based on teach-key point normal weighting.

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

[0040] 1. This invention establishes a shared control method that integrates robot model and state feedback. Position is controlled by computational torque, and attitude is controlled by feedback torque. Contact constant force impedance control is integrated into the position computational torque control framework, achieving coordinated control of robot position, attitude, and contact force. Specifically, when obtaining the desired attitude, the path point normal vector is calculated based on the normal vectors of key points on the workpiece surface, and finally, the desired attitude is calculated, without relying on the workpiece model. During attitude control, only the robot control level is involved, without relying on the robot system model. This reduces the dependence on the workpiece and system models while ensuring the accuracy of system pose tracking.

[0041] 2. This 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, it uses density clustering algorithm to determine key points of the teaching trajectory based on the teaching data and calculates the normal of key points using vector cross product method. In the curved surface wiping task, the K-nearest neighbor algorithm is used to determine local key points around the path points and the normal of the path points is estimated in real time by Gaussian weighting of the normal of the key points, so as to achieve fast and accurate workpiece surface normal estimation and robot desired posture generation. Attached Figure Description

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

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

[0044] Figure 3 The diagram shows a workpiece normal estimation model based on teaching key point normal weighting constructed according to a preferred embodiment of the present invention, wherein (a) is the task path point normal weighting model and (b) is the robot desired pose generation model.

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

[0046] Figure 5 The experimental platform for wiping with a robotic developer is constructed according to a preferred embodiment of the present invention, wherein (a) is the experimental setup and (b) is a partial view of the wiping trajectory on the curved surface.

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

[0048] Figure 7This is a diagram showing the constant force control results obtained according to a preferred embodiment of the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be 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 illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can 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 on a curved workpiece based on teaching key point normal weighting, characterized in that the method includes the following steps:

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

[0052] The operator drags the robot along a taught zigzag trajectory, such as... Figure 2 As shown in (a), the robot end effector maintains continuous contact with the workpiece and lingers at the turning points of the polygonal trajectory for a relatively long time to obtain more accurate turning point position information. Position and velocity data of the robot end effector are collected at a fixed frequency throughout the entire drag teaching process to determine the positions of key teaching points and calculate their normal vectors.

[0053] Since the data points are more densely distributed at the turning points of the polyline trajectory, the density-based clustering algorithm DBSCAN is used to determine the locations of the turning points, i.e., the key teaching points, based on the position and velocity data during the drag-and-drop teaching process. Where i = 1, 2, ..., K, and K is the number of key points.

[0054] Obtain key point location information p i Then, design a sphere with radius R centered on the key point. i Virtual spheres, such as Figure 2 As shown in (b). In one embodiment of the invention, the radius of the virtual sphere is designed to be 0.01m. The two intersection points p between the teaching trajectory and the virtual sphere are calculated. i,1 and p i,2 ;

[0055] Constructing proximity vectors and far away vector

[0056] At this point, the normal vector of the key point can be obtained through the cross product of vectors. We obtain S(v) i,departure ) is an antisymmetric matrix. This refers to the keypoint normal vector. It's worth noting that the calculated keypoint normals may have inconsistent orientations. A simple conditional statement can be used to uniformly set the keypoint normals to face outwards from the workpiece.

[0057] S2 obtains local key points based on the location of the task path points, estimates the path point normals using a key point normal weighting method, and simultaneously calculates the robot's desired pose for robot pose control.

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

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

[0060]

[0061] in, The normal vector of the path point. Let W be the matrix formed by the normal vectors of the local key points, and ω be the weight vector. i Here are the Gaussian weights, k is the number of local keypoints, and p... c For end effector position information, p i σ represents the location information of key points, and σ is the radius of the Gaussian function.

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

[0063]

[0064] S3 designs a shared control method that fuses robot models and states to achieve coordinated control of the robot's end effector position, attitude, and contact force. Figure 4 As shown.

[0065] Based on the robot's joint space dynamics model, its Cartesian space dynamics model is established.

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

[0067]

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

[0069] Based on the relationship between robot joint space velocity and Cartesian space velocity Relationship between joint space torque and Cartesian space torque The Cartesian space dynamics model of the n-DOF rotary joint robot can be obtained as follows:

[0070]

[0071] in, For the robot's Cartesian space velocity, For Cartesian space linear velocity, For Cartesian space angular velocity. These represent the Cartesian space inertia matrix, the Coriolis force and centrifugal force matrix, and the gravity vector, respectively, with F being the Cartesian space control torque. e Let M be the Cartesian space environment torque measured by the sensor. For simplicity, it will be referred to as M thereafter. x C x G x Each matrix is ​​represented in the form of .

[0072] Design a shared control framework that integrates robot model and state to achieve operator position drag control, robot posture autonomous control, and robot contact constant force control.

[0073] The shared control torque for fusion of the robot model and state is designed as follows:

[0074]

[0075] By designing variable F p and F o This allows for the control of the robot's position and orientation in Cartesian space. Substituting the shared control torque into the robot's dynamic equations yields...

[0076] Decomposition yields It can be further broken down into robot position control. Robot posture control

[0077] For robot position control, a computational torque control method is employed to improve the robot's ability to regulate position and velocity during motion. The position control torque in the robot's shared control framework is designed as follows:

[0078]

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

[0080] Simultaneously, constant force impedance control is integrated into the computational torque control framework, achieving a unified approach to position and force control. Therefore, based on position control, a force error term is added to achieve position control in the motion direction and constant force control in the contact direction, as detailed below:

[0081]

[0082] Where, ΔF=F ep -F d For force error, F ep For F e The first three elements, F d For the desired contact force, S p =diag(0,0,1) is the constant force direction selection matrix. Constant force control is selected in the robot's z-direction, and the control parameter for this direction is k. z =0, b z = b + Δb(t).

[0083]

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

[0085] For robot posture control, a feedback control method is adopted 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 For the robot's angular velocity in Cartesian space, This represents the robot's Cartesian space attitude error. The Cartesian space attitude error is calculated using the rotation matrix of the end effector contact point and the desired attitude. It is a positive definite control matrix.

[0088] The S4 operator drags the robot to perform a curved surface wiping task. The workpiece normal is estimated in real time based on the position of the robot's contact point. Combined with the proposed shared control method, the robot's posture is autonomously controlled and the contact constant force is controlled.

[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 In addition to the robot control frequency and the force sensor sampling frequency, it performs the task of wiping the curved workpiece with developer.

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

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

[0092]

[0093] Where k z =0, b z The design constant force control law is met. In the design constant force control law, b = 10, σ = 0.005, and ε = 10. -8 Simultaneously set the attitude control coefficient. and The robot control frequency is 200Hz, and the force sensor sampling frequency is 200Hz.

[0094] The operator drags the robot to keep its end effector in continuous contact with the environment, and along... Figure 5The trajectory shown in (b) is used for wiping a curved workpiece. During the task, based on the workpiece normal estimation model weighted by the normals of the taught key points, and combined with the contact position information between the robot end effector and the workpiece, the desired posture of the robot end effector contact point is calculated in real time. Finally, this rotation matrix and the 5N desired contact force information are input into the shared control framework, enabling the robot end effector normal and the workpiece normal to achieve autonomous alignment control, while simultaneously achieving constant contact force control. The normal tracking results are as follows: Figure 6 As shown, the constant force tracking situation is as follows: Figure 7 As shown. This completes the final wiping task for the entire curved workpiece.

[0095] Those skilled in the art will readily understand 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 within the scope of protection of the present invention.

Claims

1. A method for shared control of a surface workpiece wiping robot based on teaching key point normal weighting, characterized in that, The method includes the following steps: The end effector of the wiping robot is dragged to form a teaching trajectory, and the key points on the teaching trajectory and the normal vectors of each key point are determined. The desired pose of the robot's end effector on the wiping trajectory is calculated using key points and their normal vectors on the teaching trajectory. Solve the human-machine shared control torque model and robot dynamics model to obtain the position and force control torque and attitude control torque, so that the actual posture of the robot end on the wiping trajectory coincides with the desired 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 attitude control torque to realize human-machine shared control. The human-machine shared control torque model and robot dynamics model are as follows: in, For controlling torque in Cartesian space, The Cartesian space environment torque measured by the sensor. For position and force control torque, For attitude control torque, For Cartesian space pose, For Cartesian space velocity, For Cartesian space acceleration, , , These are the Cartesian space inertia matrix, the Coriolis force and centrifugal force matrix, and the gravity vector, respectively. The position and force control torque are as follows: in, For position and force control torque, The inertial matrix in Cartesian space. For the robot's Cartesian spatial position error, For Cartesian space velocity error, For the expected acceleration in Cartesian space, The position and force control positive definite stiffness matrix, The position and force control positive definite damping matrix, For force error, Choose a matrix for the direction of constant force. It is a zero vector.

2. The method for shared control of a surface workpiece wiping robot based on teaching key point normal weighting as described in claim 1, characterized in that, The attitude control torque is as follows: in, For attitude control torque, It is a zero vector. For Cartesian space attitude error, For Cartesian space angular velocity, The attitude control positive definite stiffness matrix is... This is the positive definite damping matrix for attitude control.

3. The method for shared control of a surface workpiece wiping robot based on teaching key point normal weighting as described in claim 1, characterized in that, The process of obtaining the normal vectors of the key points is as follows: Obtain the location of the key points, and calculate the two intersection points between the teaching trajectory and the virtual sphere with the key points as the center. and ; Calculate the normal vector of the key point according to the following formula: in, For the first The normal vector of each key point It is the first Location information of key points and For the two intersections of the teaching trajectory and the virtual ball, Key point The nearest vector at that point, Key point The vector away from the point.

4. The method for shared control of a surface workpiece wiping robot based on teaching key point normal weighting as described in claim 3, characterized in that, The process of obtaining the desired pose of the robot's end effector is as follows: Obtain a preset number of key points around a path point as local key points of that path point; The normal vector of a path point is obtained by weighted calculation using the normal vectors of the local key points of the path point. The desired pose of the robot's end effector is calculated using the normal vectors of the path points.

5. The method for shared control of a surface workpiece wiping robot based on teaching key point normal weighting as described in claim 4, characterized in that, The formula for calculating the normal vector of the path point is as follows: in, The normal vector of the path point. The matrix formed by the normal vectors of the local key points. It is a weight vector.

6. The method for shared control of a surface workpiece wiping robot based on teaching key point normal weighting as described in claim 5, characterized in that, The weight vector The calculation formula is as follows: in, For weight vectors, For each Gaussian weight, This represents the number of local key points. For end effector position information, For key point location information, Let be the radius of the Gaussian function.

7. A method for shared control of a surface workpiece wiping robot based on teaching key point normal weighting, as described in claim 1 or 4, characterized in that, The desired posture is as follows: in, Let be the desired rotation matrix. For the robot's end effector rotation matrix, Let be the rotation transformation matrix. For the axis of rotation, The rotation angle is... , , The three elements are the axis of rotation. The value of the cosine of the rotation angle. The value is the sine of the rotation angle.

8. A system for shared control of a surface workpiece wiping robot based on teaching key point normal weighting, characterized in that, The system includes an actuator for performing a method for shared control of a surface workpiece wiping robot based on taught key point normal weighting, as described in any one of claims 1-7.

Images