Method for controlling uniform motion of industrial robot on curved surface

Abstract

The present invention provides a method for controlling the uniform motion of an industrial robot surface, comprising the following steps: S1. Sampling the surface of a product to be processed, obtaining surface sampling points, fitting the sampling points, obtaining feature points, and then obtaining a cubic B-spline curve based on the feature points; S2. Uniformly interpolating the cubic B-spline curve to obtain interpolation points, and obtaining control points and control point normal vectors for controlling the uniform motion of a manipulator end effector based on the interpolation points; S3. According to the control requirements and the coordinates of the control points, obtaining the posture matrix of the control points of the manipulator end effector, converting the posture matrix data into a command executable by the robot, so that the robot performs uniform curvilinear motion at a fixed frequency. Through the above method, a smoother motion trajectory with smaller speed fluctuations can be generated, effectively avoiding robot jitter, ensuring uniform motion of the robot, and helping to improve the surface processing quality of the industrial robot.

Description

Technical Field

[0001] The invention relates to the field of curved surface motion of an industrial robot, and in particular to a method for controlling the curved surface uniform motion of an industrial robot. Background Art

[0002] At present, industrial robots are increasingly used in the fields of industrial manufacturing and remanufacturing, and the control accuracy requirements for robots are also getting higher and higher. In actual robot application scenarios, the robot's target motion trajectory is often a collection of discrete points. If these discrete points are directly input into the robot controller, the resulting robot motion trajectory is not smooth and has large speed fluctuations. This uneven motion trajectory can cause the robot's end effector to shake; in addition, in various additive and subtractive surface processing operations, speed fluctuations can cause uneven subtractive depth or additive height, resulting in substandard surface processing quality.

[0003] In order to solve the above technical problems, it is urgent to propose a new technical means. Summary of the invention

[0004] In view of this, the purpose of the present invention is to provide a method for controlling the uniform motion of an industrial robot on a curved surface, which can generate a smoother motion trajectory with less speed fluctuation, effectively avoid the problem of robot arm jitter, and help improve the surface processing quality of the industrial robot.

[0005] The present invention provides a method for controlling uniform motion of an industrial robot on a curved surface, comprising the following steps:

[0006] S1. Sample the surface of the product to be processed, obtain surface sampling points, fit the sampling points, obtain feature points, and then obtain a cubic B-spline curve based on the feature points;

[0007] S2. uniformly interpolate the cubic B-spline curve to obtain interpolation points, and obtain control points for controlling the uniform motion of the end effector of the robot arm according to the interpolation points;

[0008] S3. According to the control requirements and the coordinates of the control points, the pose matrix of the control points of the end effector of the robot is obtained, and the pose matrix data is converted into executable commands for the robot, so that the robot can perform uniform curved motion at a fixed frequency.

[0009] Further, in step S1, surface sampling is performed according to the following method to obtain surface sampling points:

[0010] S11. Calibrate the camera parameters according to the robot coordinate system, establish the camera imaging geometric model, use the camera to scan the surface of the product to be processed, obtain the point cloud data relative to the robot coordinate system, enter the point cloud data of the surface of the product to be processed into a PCD point cloud file, filter the data in the PCD point cloud file, and convert the filtered PCD point cloud file into an STL file;

[0011] S12. Draw the basic processing area of ​​the surface of the product to be processed on the XY plane according to the position of the product to be processed, and then obtain the surface processing path of the product to be processed according to the processing conditions such as the printing width or the cutting width, and obtain discrete points at a certain interval on the surface processing path, where the certain interval refers to 5 to 10 times the product of the robot's working frequency cycle and the motion rate;

[0012] S13. Project the discrete points onto the triangular face in the STL file to obtain the sampling points of the surface processing trajectory, and use the normal vector of the triangular face where the sampling point is located as the normal vector of the sampling point, and save the sampling point and the normal vector data of the sampling point to a TXT file.

[0013] Further, in step S1, characteristic points and cubic B-spline curves are obtained according to the following method:

[0014] Input the sampling points in the TXT file into the fitting program to obtain the feature points, and then fit the feature points according to the fitting equation, where the fitting equation is:

[0015]

[0016] Among them, E(u) is the B-spline curve equation, u is the independent variable, the range is [0,1], P i is the characteristic point of the control curve, F i,k (u) is the k-order B-spline basis function, i represents the serial number of the basis function, and k represents the order of the basis function. The sampling point data in the TXT file is input into the fitting program to obtain the feature points, and then the feature points are brought into the B-spline curve equation to obtain the cubic B-spline curve of the product surface to be processed:

[0017] P(u)=P0*F 0,3 (u)+P1*F 1,3 (u)+P2*F 2,3 (u)+P3*F 3,3 (u)

[0018] Wherein, P(u) is the cubic B-spline curve equation of the product surface to be processed.

[0019] Further, in step S2, the cubic B-spline curve is uniformly interpolated according to the following method:

[0020] S21. Simplify the P(u) equation into a cubic curve form:

[0021] P(u)=Au 3 +Bu 2 +Cu+D

[0022] Among them, the values ​​of A, B, C, and D are determined according to the characteristic points;

[0023] S22. Set the starting point of the cubic B-spline curve to G(0,0,0), there is a point E on the curve, the u value corresponding to point E is e, the curve length at point E is s, when point E is the end point, s is the length of the cubic B-spline curve, and the curve length is calculated by numerical integration;

[0024]

[0025] Where L(e) is the curve length formula, is the variable upper limit integral, which represents the length from point E to the starting point G. e is the upper limit of the integral, and the range is [0,1]. is the derivative of P(u), represents the slope of the tangent line at point E, w i and x i is the integral weight;

[0026] S23. Set the movement rate of the end effector of the robot manipulator arm to v, and obtain the time step Δt=1 / f of the movement of the end effector of the robot manipulator arm according to the fixed frequency f of the movement of the end effector of the robot manipulator arm. Take the time step Δt as the unit time, and obtain the movement path of the end effector of the robot manipulator arm along the cubic B-spline curve in unit time. Take the length of the movement path in unit time as the interval distance Δs of each interpolation point, Δs=v·Δt;

[0027] S24. When point E is the end point, s is the length of the cubic B-spline curve, and the length range of the cubic B-spline curve is set to [0, s]. Within [0, s], uniform interpolation is performed with Δs as the interval distance.

[0028] Further, the control point coordinates and control point normal vectors are obtained according to the following method:

[0029] According to Δs to e=L -1 (s) Solve and substitute the obtained e value into the cubic B-spline curve P(u) = Au 3 +Bu 2 +Cu+D, we get equally spaced control points Q(x,y,z);

[0030] According to the e value and the normal vectors T1(TX1,TY1,TZ1) and T2(TX2,TY2,TZ2) of the two adjacent sampling points of the control point, the normal vector of the control point is M(Mx,My,Mz)=(1-u)*T1(TX1,TY1,TZ1)+u*T2(TX2,TY2,TZ2).

[0031] Further, in step S3, the pose matrix of the end effector of the robot arm is obtained according to the following method:

[0032] The robot's posture matrix T is as follows:

[0033]

[0034] Among them, p is the position vector, expressed by the position coordinates of the control point, R is the rotation matrix, and the first, second, and third columns of the rotation matrix represent the unit direction vectors of the pose matrix of each control point in the x, y, and z directions, respectively, which are determined according to the control requirements; the rotation matrix is ​​combined with the control point coordinates to obtain the pose matrix of the control point, and the pose matrix data of the control point is saved in the T.txt file. The data in the T.txt file is converted into executable commands for the robot to make the robot perform uniform curved motion at a fixed frequency f.

[0035] Further, the rotation matrix R is obtained according to the following method:

[0036] S31. According to control requirement 1: the x-axis of the robot end effector is in the same straight line with the movement direction of the robot end, and the x-direction of the rotation matrix is ​​set to be in the same straight line with the movement direction of the robot end. The x-direction is [r xx ,r yx ,r zx ]=||(Q i+1 -Q i ) / l x ||, where l x =||Q i+1 -Q i ||;

[0037] S32. According to control requirement 2: limit the range of change of the normal vector angles of adjacent control points and the range of change of the normal vector angles of the control points and the average normal vector, so that the end effector of the robot arm maintains stable movement:

[0038] S321. Limit the included angle dθ of the normal vectors of adjacent control points, where dθ = arccos(M1·M2 / |M1|·|M2|) < dθ0, and dθ0 is an empirical value determined according to the model of the robot and the effect of the simulation experiment. There are two adjacent control point normal vectors M3(Mx3, My3, Mz3) and M4(Mx4, My4, Mz4). According to the first control point normal vector M3(Mx3, My3, Mz3), determine the correction value M′4(Mx′4, My′4, Mz′4) of the second control point normal vector. When the included angle between the two adjacent control point normal vectors is less than dθ0, the second control point normal vector is M4(Mx4, My4, Mz4). When the included angle between the two adjacent control point normal vectors is greater than dθ0, limit the included angle to dθ0 and calculate the corrected control point normal vector M′4(Mx′4, My′4, Mz′4). The calculation process is as follows:

[0039] dθ0 = arccos(M3·M′4 / |M3|·|M′4|)

[0040] M3·M′4 = cos(dθ0)

[0041] M4·M′4 = cos(dθ - dθ0)

[0042] ||M′4|| = 1

[0043] S322. After limiting the included angle between the normal vectors of adjacent control points to be less than dθ0, limit the included angle between the control point normal vector M′4(Mx′4, My′4, Mz′4) and the average normal vector m(mx, my, mz), where dδ = arccos(M′4·m / |M′4|·|m|) < dδ0, and dδ0 is an empirical value determined according to the model of the robot and the effect of the simulation experiment. According to the average normal vector, determine the correction value M″4(Mx″4, My″4, Mz″4) of the control point normal vector again. When the included angle between the control point normal vector M′4(Mx′4, My′4, Mz′4) and the average normal vector m(mx, my, mz) is less than dδ0, the control point normal vector is M′4(Mx′4, My′4, Mz′4). When the included angle between the control point normal vector and the average normal vector m(mx, my, mz) is greater than dδ0, limit the included angle to dδ0 and calculate the re-corrected control point normal vector M″4(Mx″4, My″4, Mz″4). The calculation process is as follows:

[0044] dδ0 = arccos(M′4·m / |M′4|·|m|)

[0045] m·M′4 = cos(dδ0)

[0046] M′4·M″4 = cos(dδ - dδ0)

[0047] ||M″4||=1

[0048] The average normal vector m (mx, my, mz) is calculated by adding the normal vectors of all control points of the cubic B-spline curve, dividing it by the total number of control points, and then normalizing it.

[0049] S323. Let the z direction of the rotation matrix be the same as the normal vector of the control point, and the z direction is [t xz ,r yz ,r zz ]=[Mx″4,My″4,Mz″4] / l z , l z is the modulus of the control point normal vector M″4(Mx″4,My″4,Mz″4);

[0050] S33. Determine the y direction of the rotation matrix by the right-hand rule of the coordinate system. xy ,r yy ,r zy ]=[r xz ,r yz ,r zz ]×[r xx ,r yx ,r zx ] / l y , where l y For vector [r xz ,r yz ,r zz ]×[r xx ,r yx ,r zx ] model.

[0051] The beneficial effects of the present invention are as follows: through the present invention, a smoother motion trajectory with smaller speed fluctuation can be generated, the problem of robot arm jitter can be effectively avoided, and the surface processing quality of the industrial robot can be improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The present invention will be further described below in conjunction with the accompanying drawings and embodiments:

[0053] Figure 1 It is a flow chart of the present invention. DETAILED DESCRIPTION

[0054] The present invention is further described in detail below:

[0055] The present invention provides a method for controlling the uniform motion of an industrial robot on a curved surface, comprising the following steps:

[0056] S1. Sample the surface of the product to be processed, obtain surface sampling points, fit the sampling points, obtain feature points, and then obtain a cubic B-spline curve based on the feature points;

[0057] S2. uniformly interpolate the cubic B-spline curve to obtain interpolation points, and obtain control points for controlling the uniform motion of the end effector of the robot arm according to the interpolation points;

[0058] S3. According to the control requirements and the coordinates of the control points, the pose matrix of the control points of the end effector of the robot is obtained, and the pose matrix data is converted into executable commands for the robot, so that the robot can perform uniform curved motion at a fixed frequency.

[0059] In this embodiment, in step S1, surface sampling is performed according to the following method to obtain surface sampling points:

[0060] S11. Calibrate the camera parameters according to the robot coordinate system, establish the camera imaging geometric model, use the camera to scan the surface of the product to be processed, obtain the point cloud data relative to the robot coordinate system, enter the point cloud data of the surface of the product to be processed into a PCD point cloud file, filter the data in the PCD point cloud file, and convert the filtered PCD point cloud file into an STL file;

[0061] S12. Draw the basic processing area of ​​the surface of the product to be processed on the XY plane according to the position of the product to be processed, and then obtain the surface processing path of the product to be processed according to the processing conditions such as the printing width or the cutting width, and obtain discrete points at a certain interval on the surface processing path, where the certain interval refers to 5 to 10 times the product of the robot's working frequency cycle and the motion rate;

[0062] S13. Project the discrete points onto the triangular face in the STL file to obtain the sampling points of the surface processing trajectory, and use the normal vector of the triangular face where the sampling point is located as the normal vector of the sampling point, and save the sampling point and the normal vector data of the sampling point to a TXT file.

[0063] Through the above method, sampling points that are more closely aligned with the surface of the product to be processed can be obtained.

[0064] In this embodiment, in step S1, characteristic points and cubic B-spline curves are obtained according to the following method:

[0065] The sampling points in the TXT file are input into the fitting program to obtain the characteristic points, and then the characteristic points are fitted according to the fitting equation, wherein the fitting program is the prior art, and the fitting equation is:

[0066]

[0067] Among them, E(u) is the B-spline curve equation, u is the independent variable, the range is [0,1], Pi is the characteristic point of the control curve, F i,k (u) is the k-order B-spline basis function, which is an existing formula, i represents the serial number of the basis function, k represents the order of the basis function, the sampling point data in the TXT file is input into the fitting program to obtain the characteristic points, and then the characteristic points are brought into the total equation of the B-spline curve to obtain the cubic B-spline curve of the product surface to be processed:

[0068] P(u)=P0*F 0,3 (u)+P1*F 1,3 (u)+P2*F 2,3 (u)+P3*F 3,3 (u)

[0069] Among them, P(u) is the cubic B-spline curve equation of the product surface to be processed. Since the cubic B-spline curve will produce distortion at both ends of the curve, it is necessary to add 5 sacrifice points at both ends of the curve and delete the distorted curve. Through the above method, a smoother surface processing path with less speed fluctuation can be generated.

[0070] In this embodiment, the length range of the cubic B-spline curve is uniformly interpolated according to the following method to obtain control points:

[0071] S21. Simplify the P(u) equation into a cubic curve form:

[0072] P(u)=Au 3 +Bu 2 +Cu+D

[0073] Wherein, the values ​​of A, B, C, and D are determined according to the characteristic points; wherein, converting P(u) into a cubic curve form is achieved by an existing method;

[0074] S22. Set the starting point of the cubic B-spline curve to G(0,0,0), there is a point E on the curve, the u value corresponding to point E is e, the curve length at point E is s, that is, the length from the starting point of the curve to point E is s, when point E is the end point of the curve, s is the length of the cubic B-spline curve, and the curve length is calculated by numerical integration;

[0075]

[0076]

[0077] Where L(e) is the curve length formula, is the variable upper limit integral, which represents the length from point E to the starting point G. e is the upper limit of the integral, and the range is [0,1]. is the derivative of P(u), represents the slope of the tangent line at point E, They represent the derivatives of point E in the x, y, and z directions respectively;

[0078] Among them, the numerical integration is realized by Gauss-lengendre integration, because is a second-order function, and the integral weight of the cubic B-spline curve is w i and x i， w i and x i The value can be obtained by consulting the Gauss-lengendre integral weight table;

[0079] S23. Set the movement rate of the end effector of the robot manipulator arm to v, and obtain the time step Δt=1 / f of the movement of the end effector of the robot manipulator arm according to the fixed frequency f of the movement of the end effector of the robot manipulator arm. Take the time step Δt as the unit time, and obtain the movement path of the end effector of the robot manipulator arm along the cubic B-spline curve in unit time. Take the length of the movement path in unit time as the interval distance Δs of each interpolation point, Δs=v·Δt;

[0080] S24. When point E is the end point, s is the length of the cubic B-spline curve, and the length range of the cubic B-spline curve is set to [0, s]. Within [0, s], uniform interpolation is performed with Δs as the interval distance.

[0081] Through the above method, a more accurate curve length can be obtained, and accurate uniform interpolation can be performed.

[0082] In this example, the control point coordinates and control point normal vectors are obtained according to the following method:

[0083] According to Δs to e=L -1 (s) Solve and substitute the obtained e value into the cubic B-spline curve P(u) = Au 3 +Bu 2 +Cu+D, we get equally spaced control points Q(x,y,z), where the process of solving the E value is the prior art and will not be described in detail here;

[0084] According to the e value and the normal vectors T1(TX1,TY1,TZ1) and T2(TX2,TY2,TZ2) of the two adjacent sampling points of the control point, the normal vector of the control point is M(Mx,My,Mz)=(1-u)*T1(TX1,TY1,TZ1)+u*T2(TX2,TY2,TZ2).

[0085] Since the cubic B-spline curve is a smooth curve composed of segmented curves, and the cubic B-spline curve is determined by four adjacent feature points, the curve equation of each segment changes with the coordinates of the feature points. Therefore, segment processing is required when interpolating control points. Through the above method, the equally spaced control points and control point normal vectors of the surface path of the product to be processed can be accurately obtained.

[0086] In this embodiment, in step S3, the pose matrix of the end effector of the robot arm is obtained according to the following method:

[0087] The robot's posture matrix T is as follows:

[0088]

[0089] Among them, p is the position vector, expressed by the position coordinates of the control point, R is the rotation matrix, and the first, second, and third columns of the rotation matrix represent the unit direction vectors of the pose matrix of each control point in the x, y, and z directions, respectively. It is determined according to the control requirements and is specifically expressed as the components of the normal vector of each control point in the x, y, and z-axis directions of the robot coordinate system. Each component is normalized to obtain the unit direction vector of the pose matrix of each control point in the x, y, and z directions; the rotation matrix is ​​combined with the control point coordinates to obtain the pose matrix of the control point, and the pose matrix data of the control point is saved in the T.txt file. The data in the T.txt file is converted into executable commands for the robot to make the robot perform uniform curved motion at a fixed frequency f.

[0090] Through the above method, the position and posture matrix of each control point can be accurately obtained, so that the end effector of the robot arm can move at a uniform speed.

[0091] In this embodiment, the rotation matrix R is obtained according to the following method:

[0092] S31. According to control requirement 1: the x-axis of the robot end effector is in the same straight line with the movement direction of the robot end, and the x-direction of the rotation matrix is ​​set to be in the same straight line with the movement direction of the robot end. The x-direction is [r xx ,r yx ,r zx ]=||(Q i+1 -Q i ) / l x ||, where l x =|||Q i+1 -Q i ||;

[0093] S32. According to control requirement 2: limit the range of change of the normal vector angles of adjacent control points and the range of change of the normal vector angles of the control points and the average normal vector, so that the end effector of the robot arm maintains stable movement:

[0094] S321. Limit the included angle dθ of the normal vectors of adjacent control points, where dθ = arccos(M1·M2 / |M1|·|M2|) < dθ0, and dθ0 is an empirical value determined according to the model of the robot and the effect of the simulation experiment; there are two adjacent control point normal vectors M3(Mx3, My3, Mz3) and M4(Mx4, My4, Mz4). According to the first control point normal vector M3(Mx3, My3, Mz3), determine the correction value M′4(Mx′4, My′4, Mz′4) of the second control point normal vector. When the included angle of the two adjacent control point normal vectors is less than dθ0, the second control point normal vector is M4(Mx4, My4, Mz4). When the included angle of the two adjacent control point normal vectors is greater than dθ0, limit the included angle to dθ0, and calculate the corrected control point normal vector M′4(Mx′4, My′4, Mz′4). The calculation process is as follows:

[0095] dθ0 = arccos(M3·M′4 / |M3|·|M′4|)

[0096] M3·M′4 = cos(dθ0)

[0097] M4·M′4 = cos(dθ - dθ0)

[0098] ||M′4|| = 1

[0099] S322. After limiting the included angle of the normal vectors of adjacent control points to be less than dθ0, limit the included angle between the control point normal vector M′4(Mx′4, My′4, Mz′4) and the average normal vector m(mx, my, mz), where dδ = arccos(M′4·m / |M′4|·|m|) < dδ0, and dδ0 is an empirical value determined according to the model of the robot and the effect of the simulation experiment. According to the average normal vector, determine the correction value M″4(Mx″4, My″4, Mz″4) of the control point normal vector again; when the included angle between the control point normal vector M′4(Mx′4, My′4, Mz′4) and the average normal vector m(mx, my, mz) is less than dδ0, the control point normal vector is M′4(Mx′4, My′4, Mz′4). When the included angle between the control point normal vector and the average normal vector m(mx, my, mz) is greater than dδ0, limit the included angle to dδ0, and calculate the re-corrected control point normal vector M″4(Mx″4, My″4, Mz″4). The calculation process is as follows:

[0100] dδ0 = arccos(M′4·m / |M′4|·|m|)

[0101] m·M′4 = cos(dδ0)

[0102] M′4·M″4 = cos(dδ - dδ0)

[0103] ||M″4||=1

[0104] The average normal vector m (mx, my, mz) is calculated by adding the normal vectors of all control points of the cubic B-spline curve, dividing it by the total number of control points, and then normalizing it.

[0105] S323. Let the z direction of the rotation matrix be the same as the normal vector of the control point, and the z direction is [r xz ,r yz ,r zz ]=[Mx″4,My″4,Mz″4] / l z , l z is the modulus of the control point normal vector M″4(Mx″4,My″4,Mz″4);

[0106] S33. Determine the y direction of the rotation matrix by the right-hand rule of the coordinate system. xy ,r yy ,r zy ]=[r xz ,r yz ,r zz ]×[r xx ,r yx ,r zx ] / l y , where l y For vector [r xz ,r yz ,r zz ]×[r xx ,r yx ,r zx ] model.

[0107] The above method can avoid interference and jitter problems caused by excessive changes in the pose matrix to the greatest extent.

[0108] The present invention is further described in detail below in conjunction with the ABB IRB1200 industrial robot:

[0109] The depth camera parameters are calibrated according to the robot coordinate system, and the camera imaging geometric model is established. The depth camera is used to shoot the surface of the product to be processed, and the point cloud data relative to the robot coordinate system is obtained. The point cloud data is saved in the point cloud file surface.pcd, and the data in the point cloud file surface.pcd is filtered, and the filtered data is saved in the surface.stl file; the basic processing area is drawn on the xy plane according to the position of the product to be processed, and then the processing path of the product to be processed is drawn according to the processing conditions; the processing rate of the end effector of the robot manipulator is set to 0.05m / s, and the execution frequency of the fixed robot is 250Hz, then the period is 0.004s, and the motion path within the unit period of the robot is taken as the length, and the length is 2×10 -4 m, five times the length 1×10 -3 m is the interval, discrete points are taken on the processing path, and the discrete points are projected onto each triangle patch in the surface.stl file, so as to obtain the sampling point coordinates of the surface processing path and the corresponding triangle patch normal vector as the sampling point normal vector, and the sampling point coordinates and sampling point normal vector of the surface processing path are saved in P_samp.txt; the data in P_samp.txt are input into the existing fitting program to obtain the feature points, and then the feature points are fitted into a cubic B-spline curve; since the cubic B-spline curve will produce distortion at both ends of the curve, it is necessary to add 5 sacrifice points at both ends of the curve, and delete the distorted curve.

[0110] According to the curve length formula and numerical integration, the length of the cubic B-spline curve is obtained. The length range of the cubic B-spline curve is set to [0, s]. Since the processing rate of the robot arm end effector is 0.05 m / s and the execution frequency of the fixed robot is 250 Hz, the period is 0.004 s, and the spacing between two adjacent interpolation points is Δs = 2 × 10 -4 m, interpolate at intervals of Δs within the length of the cubic B-spline curve, and calculate the u value corresponding to each interpolation point on the cubic B-spline curve according to Δs and the curve length formula. Substitute the u value back into the cubic B-spline curve equation to obtain the coordinates of the control point, and then calculate the normal vector of the control point based on the calculated u value and the normal vectors of the two sampling points adjacent to the control point.

[0111] Since the control point list only contains the position information of the end effector, when the end effector of the robot arm moves at a constant speed, it is necessary to obtain the pose matrix of the end effector control point. i The pose matrix T of [x,y,z] i Example:

[0112] The robot's posture matrix T is as follows:

[0113]

[0114] The x, y, and z direction vectors of the pose matrix are determined according to the control requirements. The x, y, and z direction vectors are unitized, and then the pose matrix of each control point is obtained by combining the position coordinates of the control points.

[0115] The rotation matrix of the end effector of the robotic arm is taken according to the control requirements. Among them, the x direction of the rotation matrix is consistent with the direction of the end effector of the robotic arm, and the x direction [r xx , r yx , r zx = ||(Q i+1 - Q i ) / l x ||, where l x = ||Q i+1 - Q i ||; the z direction of the rotation matrix is consistent with the normal vector of the restricted control point. First, the included angle between the normal vectors of two adjacent control points is restricted to dθ = arccos(M3·M′4 / |M3|·|M′4|) < dθ0 to obtain the corrected normal vector M′4 (Mx′4, My′4, Mz′4) of the control point; then, the included angle between the normal vector of the control point and the average normal vector is restricted to dδ = arccos(M′4·m / |M′4|·|m|) < dδ0 to obtain the further corrected normal vector M″4 (Mx″4, My″4, Mz″4) of the control point. The z direction [r xz , r yz , r zz = [Mx″4, My″4, Mz″4] / l z ; the y direction of the rotation matrix is determined according to the right - hand rule of the coordinate system. The y direction [r xy , r yy , r zy = [r xz , r yz , r zz × [r xx , r yx , r zx / l y , and the position vector is represented by the coordinates of the control points. Thus, the pose matrices of all control points are obtained and saved to T.txt.

[0116] The pose matrix T.txt file is converted into commands executable by the robot, enabling the end effector of the robot's robotic arm to move uniformly along the surface of the product to be processed at a frequency of 250 Hz.

[0117] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution of the present invention, which should be included in the scope of the claims of the present invention.

Claims

1. A method for controlling uniform motion of an industrial robot on a curved surface, characterized in that: The following steps are involved: S1. Sample the surface of the product to be processed, obtain surface sampling points, fit the sampling points, obtain feature points, and then obtain a cubic B-spline curve based on the feature points; S2. uniformly interpolate the cubic B-spline curve to obtain interpolation points, and obtain control points for controlling the uniform motion of the end effector of the robot arm according to the interpolation points; The cubic B-spline curve is uniformly interpolated according to the following method: S21. Simplify the P(u) equation into a cubic curve form: P(u)=Au 3 +Bu 2 +With+D Among them, the values ​​of A, B, C, and D are determined according to the characteristic points; S22. Set the starting point of the cubic B-spline curve to G(0,0,0), there is a point E on the curve, the u value corresponding to point E is e, the curve length at point E is s, when point E is the end point, s is the length of the cubic B-spline curve, and the curve length is calculated by numerical integration; Where L(e) is the curve length formula, is the variable upper limit integral, which represents the length from point E to the starting point G. e is the upper limit of the integral, and the range is [0,1]. is the derivative of P(u), represents the slope of the tangent line at point E, w i and x i is the integral weight; S23. Set the movement rate of the end effector of the robot manipulator arm to v, and obtain the time step Δt=1 / f of the movement of the end effector of the robot manipulator arm according to the fixed frequency f of the movement of the end effector of the robot manipulator arm. Take the time step Δt as the unit time, and obtain the movement path of the end effector of the robot manipulator arm along the cubic B-spline curve in unit time. Take the length of the movement path in unit time as the interval distance Δs of each interpolation point, Δs=v·Δt; S24. When point E is the end point, s is the length of the cubic B-spline curve, and the length range of the cubic B-spline curve is set to [0, s]. Within [0, s], uniform interpolation is performed with Δs as the interval distance; S3. According to the control requirements and the coordinates of the control points, the pose matrix of the control points of the end effector of the robot arm is obtained, and the pose matrix data is converted into a command executable by the robot, so that the robot performs uniform curvilinear motion at a fixed frequency; The pose matrix of the end effector of the robot arm is obtained according to the following method: The robot's posture matrix T is as follows: Among them, p is the position vector, which is expressed by the position coordinates of the control point, R is the rotation matrix, and the first, second, and third columns of the rotation matrix represent the unit direction vectors of the pose matrix of each control point in the x, y, and z directions, respectively, which are determined according to the control requirements; the rotation matrix is ​​combined with the control point coordinates to obtain the pose matrix of the control point, and the pose matrix data of the control point is saved in the T.txt file, and the data in the T.txt file is converted into executable commands for the robot to make the robot perform uniform curved motion at a fixed frequency f; The rotation matrix R of each control point is obtained according to the following method: S31. According to control requirement 1: the x-axis of the robot end effector is in the same straight line with the movement direction of the robot end, and the x-direction of the rotation matrix is ​​set to be in the same straight line with the movement direction of the robot end. The x-direction is [r xx ,r yx ,r zx ]=||(Q i+1 -Q i ) / l x ||, where l x =||Q i+1 -Q i ||; S32. According to control requirement 2: limit the range of change of the normal vector angles of adjacent control points and the range of change of the normal vector angles of the control points and the average normal vector, so that the end effector of the robot arm maintains stable movement: S321. The method of restricting the included angle dθ of the normal vectors of adjacent control points is dθ = arccos(M1·M2 / |M1|·|M2|) < dθ0, where dθ0 is an empirical value determined according to the model of the robot and the effect of the simulation experiment; there are two adjacent control point normal vectors M3(Mx3, My3, Mz3) and M4(Mx4, My4, Mz4). According to the first control point normal vector M3(Mx3, My3, Mz3), the correction value M′4(Mx′4, My′4, Mz′4) of the second control point normal vector is determined. When the included angle of the two adjacent control point normal vectors is less than dθ0, the second control point normal vector is M4(Mx4, My4, Mz4). When the included angle of the two adjacent control point normal vectors is greater than dθ0, the included angle is restricted to dθ0, and the corrected control point normal vector M′4(Mx′4, My′4, Mz′4) is calculated. The calculation process is as follows: dθ0 = arccos(M3·M'4 / |M3|·|M'4|) M3·M'4 = cos(dθ0) M4·M'4 = cos(dθ - dθ0) ||M'4||＝1 S322. After restricting the included angle of the normal vectors of adjacent control points to be less than dθ0, the included angle between the control point normal vector M′4(Mx′4, My′4, Mz′4) and the average normal vector m(mx, my, mz) is restricted. dδ = arccos(M′4·m / |M′4|·|m|) < dδ0, where dδ0 is an empirical value determined according to the model of the robot and the effect of the simulation experiment. According to the average normal vector, the correction value M″4(Mx″4, My″4, Mz″4) of the control point normal vector is determined again; when the included angle between the control point normal vector M′4(Mx′4, My′4, Mz′4) and the average normal vector m(mx, my, mz) is less than dδ0, the control point normal vector is M′4(Mx′4, My′4, Mz′4). When the included angle between the control point normal vector and the average normal vector m(mx, my, mz) is greater than dδ0, the included angle is restricted to dδ0, and the control point normal vector M″4(Mx″4, My″4, Mz″4) after re - correction is calculated. The calculation process is as follows: dδ0 = arccos(M'4·m / |M'4|·|m|) m·M'4 = cos(dδ0) M'4·M”4 = cos(dδ - dδ0) ||M”4||＝1 Among them, the average normal vector m(mx, my, mz) is obtained by adding all the control point normal vectors of the cubic B - spline curve, dividing by the total number of control points, and then performing normalization calculation; S323. Let the z direction of the rotation matrix be the same as the normal vector of the control point, and the z direction is [r xz ,r yz ,r zz ]=[Mx″4,My″4,Mz″4] / l z , l z is the modulus of the control point normal vector M″4(Mx″4,My″4,Mz″4); S33. Determine the y direction of the rotation matrix by the right-hand rule of the coordinate system. xy ,r yy ,r zy ]=[r xz ,r yz ,r zz ]×[r xx ,r yx ,r zx ] / l y , where l y For vector [r xz ,r yz ,r zz ]×[r xx ,r yx ,r zx ] model.

2. The method for controlling the uniform motion of an industrial robot on a curved surface according to claim 1, characterized in that: In step S1, the following method is used for surface sampling to obtain surface sampling points: S11. Calibrate the camera parameters according to the robot coordinate system, establish a camera imaging geometric model, use the camera to scan the surface of the product to be processed, obtain the point cloud data of the surface of the product to be processed relative to the robot coordinate system, input the point cloud data of the surface of the product to be processed into a PCD point cloud file, filter the data in the PCD point cloud file, and convert the filtered PCD point cloud file into an STL file; S12. Draw the basic processing area of ​​the surface of the product to be processed on the XY plane according to the position of the product to be processed, and then obtain the surface processing path of the product to be processed according to the processing conditions such as the printing width or the cutting width, and obtain discrete points at a certain interval on the surface processing path, where the certain interval refers to 5 to 10 times the product of the robot's working frequency cycle and the motion rate; S13. Project the discrete points onto the triangular face in the STL file to obtain the sampling points of the surface processing trajectory, and use the normal vector of the triangular face where the sampling point is located as the normal vector of the sampling point, and save the sampling point and the normal vector data of the sampling point to a TXT file.

3. The method for controlling the uniform motion of an industrial robot on a curved surface according to claim 2, characterized in that: The characteristic points and cubic B-spline curve are obtained according to the following method: Input the sampling points in the TXT file into the fitting program to obtain the feature points, and then fit the feature points according to the fitting equation, where the fitting equation is: Among them, E(u) is the B-spline curve equation, u is the independent variable, the range is [0,1], P i is the characteristic point of the control curve, F i,k (u) is the k-order B-spline basis function, i represents the serial number of the basis function, and k represents the order of the basis function. The sampling point data in the TXT file is input into the fitting program to obtain the feature points, and then the feature points are brought into the B-spline curve equation to obtain the cubic B-spline curve of the product surface to be processed: P(u)=P0*F 0,3 (u)+P1*F 1,3 (u)+P2*F 2,3 (u)+P3*F 3,3 (in) Wherein, P(u) is the cubic B-spline curve equation of the product surface to be processed.

4. The method for controlling the uniform motion of an industrial robot on a curved surface according to claim 1, characterized in that: The control point coordinates and control point normal vectors are obtained as follows: According to Δs to e=L -1 (s) Solve and substitute the obtained e value into the cubic B-spline curve P(u) = Au 3 +Bu 2 +Cu+D, we get equally spaced control points Q(x,y,z); According to the e value and the normal vectors T1(TX1,TY1,TZ1) and T2(TX2,TY2,TZ2) of the two adjacent sampling points of the control point, the normal vector of the control point is M(Mx,My,Mz)=(1-u)*T1(TX1,TY1,TZ1)+u*T2(TX2,TY2,TZ2).

Images