High-precision anti-disturbance SCARA robot control method for precision assembly

Abstract

The invention relates to the technical field of robot control, in particular to a high-precision anti-disturbance SCARA robot control method for precision assembly. The method comprises the following steps: searching a working path of a mechanical arm by utilizing an ant colony algorithm; the working paths are arranged according to the high-low sequence of scores; the optimal working path is used as a preferred target path of the mechanical arm; the mechanical arm is controlled to execute according to the preferred target path; and in the execution process, if deviation occurs, judging whether to switch to other working paths or not by utilizing a fitness function. The method is used for precise assembly of the SCARA robot, so that a robot system has high anti-interference performance and high system automation degree, manual intervention is reduced, global path optimization and local dynamic adjustment are considered at the same time, and the method is suitable for precise assembly of complex environments and dynamic tasks.

Description

Technical Field

[0001] The invention relates to the technical field of robot control, and in particular to a high-precision anti-disturbance SCARA robot control method for precision assembly. Background Art

[0002] With the continuous improvement of industrial automation, robots are increasingly used in the field of precision assembly. SCARA (Selective Compliance Assembly Robot Arm) robots are widely used in high-precision assembly tasks due to their simple structure, flexible movement and high efficiency. However, due to the complex disturbance factors in the working environment (such as external force, vibration, temperature change, etc.), SCARA robots are easily affected by disturbances when performing precision assembly tasks, resulting in reduced position accuracy and unstable assembly quality.

[0003] The existing SCARA robot control system mostly uses the traditional PID control method, but due to its slow response to disturbances, it is difficult to meet the requirements of high-precision assembly. Therefore, how to improve the SCARA robot's anti-disturbance ability and ensure its high-precision operation in a disturbed environment has become a difficult problem in current technology. Summary of the invention

[0004] The present invention discloses a high-precision anti-disturbance SCARA robot control method for precision assembly, and the specific method is as follows:

[0005] Use ant colony algorithm to find the working path of the robot arm;

[0006] Calculate the score of each work path using the scoring function, and arrange the work paths in order of the score;

[0007] The optimal working path is used as the preferred target path of the robot arm;

[0008] Control the robot arm to execute according to the preferred target path;

[0009] During the execution process, if a deviation occurs, the fitness function is used to determine whether to switch to another working path.

[0010] Furthermore, the ant colony algorithm is used to find the working path of the robot arm. The specific method is as follows:

[0011] Calculate the workspace of the end effector of the robot arm, and divide the workspace into grids after obtaining it;

[0012] Initialize the data of ants and the number of path nodes in the ant colony;

[0013] The ant calculates the next possible path node to visit based on the pheromone concentration;

[0014] Use roulette to select the next node to be visited;

[0015] When all ants complete a round trip, the pheromone concentration is updated;

[0016] Until the maximum number of iterations is reached, all paths are stored as working paths.

[0017] Furthermore, the scoring function is calculated as follows:

[0018] Score=ω 1 ·L+ω 2 ·T+ω 3 ·S

[0019] In the formula, L represents the total length of the path, T represents the number of turns in the path, S represents the curvature, that is, the smoothness of the path, and ω 1 ,ω 2 ,ω 3 Represents an adjustable weight.

[0020] Furthermore, the specific method of controlling the robotic arm is as follows:

[0021] Construct a dynamic model of the robotic arm;

[0022] Design adaptive robust controllers;

[0023] Controlling a robotic arm with an adaptive robust controller.

[0024] Furthermore, the dynamic model of the robotic arm is constructed. The specific method is as follows:

[0025] The Lagrangian form of the dynamics of a mechanical system is as follows:

[0026]

[0027] Where q represents the angular displacement, represents the angular velocity, represents angular acceleration, M(q) is the inertia matrix, is the Coriolis force and centrifugal force matrix, G(q) is the gravity vector, σ(t) is the system uncertain parameter, τ(t) is the control input vector;

[0028] For three-phase current decoupling of SCARA robot joint motors:

[0029]

[0030] Among them, u d 、u q Represent the stator voltage of d-axis and q-axis respectively, i d 、i qRepresent the stator current of d-axis and q-axis respectively, R represents the stator resistance, L d , L q Represent the inductance of d-axis and q-axis respectively, ψ f represents the magnetic flux linkage, n p Represents the number of pole pairs, τ, B, J, T lp They represent the electromagnetic torque, damping coefficient, moment of inertia and load torque of the motor respectively;

[0031] And because the expression of electromagnetic torque can be expressed as:

[0032]

[0033] In vector control, i d =0, we can get:

[0034]

[0035] because

[0036]

[0037] You can get:

[0038]

[0039] Rewritten into the general Lagrangian form:

[0040]

[0041] In the formula, the matrix F is the load torque T mentioned above lp .

[0042] Furthermore, an adaptive robust controller is designed. The specific method is as follows:

[0043] The trajectory of the working path is expressed as the first-order and second-order constraint equations:

[0044]

[0045] In the formula, A is the unit matrix, and matrix b is the equivalent matrix after differential transformation of matrix c;

[0046] The designed adaptive robust controller includes:

[0047] Nominal controller τ 0 , is the controller under ideal constraints;

[0048] Robust controller τ 1 , to cope with modeling errors and initial condition deviations;

[0049] Adaptive controller τ 2, introducing adaptive parameters to adjust controller parameters in real time;

[0050] The specific formula is as follows:

[0051]

[0052] in, represents the known part of the system, κ is a positive constant, the P matrix is ​​a positive definite matrix, ∏(·) is a known function containing the adaptive parameter α, and the function ∏(·) can be linearly decomposed;

[0053] When there is no external interference and system uncertainty, That is, the controller is τ=τ 0 +τ 1 ;

[0054] When the initial conditions have no deviation and the model error can be ignored, that is, At this time, the controller is τ=τ 0 .

[0055] Furthermore, the fitness function is used to determine whether to switch to other working paths. The specific method is as follows:

[0056] The fitness function is defined as:

[0057]

[0058]

[0059] In the formula, F current represents the current path fitness, represents the fitness of other paths; d current Indicates the Euclidean distance between the current position and the nearest point on the current target path. Indicates the deviation from the current position to other paths;

[0060] Define the path switching conditions as:

[0061] when When , switch to path i; where Δ is the threshold to avoid frequent switching;

[0062] The controller performs trajectory tracking according to the currently selected path. After every set time, it evaluates the deviation and calculates the fitness to check whether the switching conditions are met. If the conditions are met, it updates the current path and replans the control input. Otherwise, it continues to execute along the current path.

[0063] Due to the adoption of the above technical solution, the present invention has the following beneficial effects:

[0064] 1. Traditional path planning methods mostly target a single path. This method uses the ant colony algorithm to generate multiple paths, sorts them through a scoring function, finds the optimal path, and retains the suboptimal path as a backup. Multi-path generation improves the robustness and flexibility of the system, and can select the most appropriate path in different situations, avoiding the limitations of single path planning.

[0065] 2. The fitness function evaluates the quality of the current path in real time, and dynamically switches to a better path based on the deviation to ensure the continuity and accuracy of path execution. In actual operation, if the environment changes or execution errors occur, the system can automatically adjust to the suboptimal path, enhancing the adaptability and fault tolerance of path execution.

[0066] 3. The designed adaptive robust controller not only improves the accuracy of path execution, but also provides robust compensation for model uncertainty and external interference. It can ensure the stability and reliability of the end effector in complex environments, and can quickly adjust to the new path state even if disturbances occur during path switching.

[0067] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] The accompanying drawings of the present invention are as follows.

[0069] Figure 1 This is a schematic diagram of the ant colony algorithm process.

[0070] Figure 2 The figure is a schematic diagram of the path switching process.

[0071] Figure 3 It is a schematic diagram of the overall process of the present invention. DETAILED DESCRIPTION

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

[0073] A high-precision, anti-disturbance SCARA robot control method for precision assembly, such as Figure 3 As shown, the specific steps are as follows:

[0074] S1. Use ant colony algorithm to find the working path of the robot arm.

[0075] Specifically, the ant colony algorithm is used to find the working path of the robot arm, such as Figure 1 As shown, the steps are as follows:

[0076] First, calculate the workspace of the end effector of the SCARA robot. After obtaining the workspace, perform grid division on it. A weighted graph with N nodes can be obtained, G=(N, A), where N represents the node set N = {1, 2, 3... n}, and A represents the edges, A = (i, j), i, j ∈ N.

[0077] Set the number of ants in the ant colony to m, the number of path nodes to n, and the mutual distance between node i and node j to (d ij ) n×n (i, j = 1, 2, 3... n). At time t, the pheromone concentration on the connection path between node i and node j is τ ij (t). At the initial moment, the pheromone concentrations between each node are the same.

[0078] Ant k (k = 1, 2, 3... m) determines its next visited node according to the pheromone concentration on the connection path between each node. The probability that ant k transfers from node i to node j at time t can be calculated by the following formula:

[0079]

[0080] where η ij (t) is the heuristic function, indicating the expected degree for the ant to transfer from node i to node j. allow k (k = 1, 2, 3... m) is the set of nodes to be visited by ant k. α and β respectively represent the importance factor of the pheromone and the importance factor of the heuristic function.

[0081] Use the roulette wheel method to select the next node to be visited.

[0082] First, calculate the cumulative probability q of each individual j :

[0083]

[0084] The superscript l represents the number of optional paths for the next step. Subsequently, randomly generate an r (0 < r < 1), and compare the sizes of all q j with r. Among them, the smallest q j greater than r is the node that the k-th ant will choose for the next step when on the i-th path.

[0085] r = rand(0, 1)

[0086] j = index{min[q j > r]}

[0087] When all ants complete a tour, the pheromone concentration on the path is:

[0088] τ ij (t+1)=(1-ρ)τ ij (t)+Δτ ij

[0089]

[0090] Among them, ρ(0<ρ<1) represents the volatility of pheromone, represents the pheromone concentration released by the kth ant on the path connecting node i and node j, Δτ ij represents the sum of the pheromone concentrations released by all ants on the path connecting point i and node j, Q (Q>0) is the pheromone constant, and L k is the total length of the path traversed by the kth ant.

[0091] S2. Calculate the score of each work path using the scoring function, and arrange the work paths in order of the score.

[0092] Sort all paths by the scoring function:

[0093] Score=ω 1 ·L+ω 2 ·T+ω 3 ·S

[0094] L represents the total length of the path, T represents the number of turns in the path, and S represents the smoothness of the path, i.e., the curvature; ω 1 ,ω 2 ,ω 3 Represents an adjustable weight.

[0095] After sorting, the top N paths with the highest scores are stored and sorted in sequence.

[0096] S3. Use the optimal working path as the preferred target path of the robot arm.

[0097] S4. Control the robot arm to execute according to the preferred target path.

[0098] Specifically, to control the robotic arm, the steps are as follows:

[0099] S41. Establish the dynamic model of the SCARA robot.

[0100] The Lagrangian form of the dynamics of a general mechanical system is as follows:

[0101]

[0102] Where q represents the angular displacement, represents the angular velocity, represents angular acceleration, M(q) is the inertia matrix, is the Coriolis force and centrifugal force matrix, G(q) is the gravity vector, σ(t) is the system uncertain parameter, and τ(t) is the control input vector.

[0103] After decoupling the three-phase current of the SCARA robot joint motor, we can get

[0104]

[0105] Among them, u d 、u q Represent the stator voltage of d-axis and q-axis respectively, i d 、i q Represent the stator current of d-axis and q-axis respectively, R represents the stator resistance, L d , L q Represent the inductance of d-axis and q-axis respectively, ψ f represents the magnetic flux linkage, n p Represents the number of pole pairs, τ, B, J, T lp They represent the electromagnetic torque, damping coefficient, moment of inertia and load torque of the motor respectively.

[0106] And because the expression of electromagnetic torque can be expressed as:

[0107]

[0108] In vector control, we use the D-axis current as zero, that is, i d =0, which simplifies the system.

[0109] get:

[0110]

[0111] From the third term in formula (1)

[0112]

[0113] You can get:

[0114]

[0115] Write equation (4) in the general Lagrangian form:

[0116]

[0117] Here the third matrix F is the load torque T mentioned above lp .

[0118] S42. Adaptive robust controller design.

[0119] The planned trajectory is expressed as first-order and second-order constraint equations:

[0120]

[0121] A is the identity matrix, and matrix b is the equivalent matrix after the differential transformation of matrix c.

[0122] The motion control of the nonlinear system is realized based on this constraint equation.

[0123] The designed controller consists of three parts: the nominal controller τ 0 , robust controller τ 1 and the adaptive controller τ 2 . Nominal controller τ 0 is the ideal controller under constraints, and the robust controller τ 1 , used to deal with modeling errors and initial condition deviations, the adaptive controller τ 2 , adaptive parameters are introduced to adjust the controller parameters in real time, and gradually reduce the impact of external disturbances on the system.

[0124]

[0125] in, represents the known part of the system, κ is a positive constant, the P matrix is ​​a positive definite matrix, Π(·) is a known function containing the adaptive parameter α, and the function Π(·) can be linearly decomposed. When there is no external interference and system uncertainty, take That is, the controller is τ=τ 0 +τ 1 , further, when the initial conditions have no deviation and the model error can be ignored, that is, At this time, the controller is τ=τ 0 .

[0126] This adaptive robust controller can effectively handle the interference of uncertainty on the system, thus improving the anti-interference ability of the system.

[0127] S5. During the execution process, if a deviation occurs, the fitness function is used to determine whether to switch to another working path.

[0128] Specifically, Figure 2 As shown, the path switching strategy is as follows:

[0129] The fitness function is defined as follows:

[0130]

[0131] F current represents the current path fitness, Represents the fitness of other paths. currentIndicates the Euclidean distance between the current position and the nearest point on the current target path. Indicates the deviation from the current position to other paths.

[0132] Path switching conditions: When When , switch to path i. Δ is the threshold to avoid frequent switching.

[0133] The controller performs trajectory tracking according to the currently selected path, evaluates the deviation and calculates the fitness every 100ms, checks whether the switching conditions are met, and if so, updates the current path and replans the control input, otherwise continues to execute along the current path.

[0134] In general, this method is used for SCARA robots for precision assembly, which makes the robot system highly anti-interference and highly automated, reduces manual intervention, and takes into account both global path optimization and local dynamic adjustment, making it suitable for precision assembly in complex environments and dynamic tasks.

[0135] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A high-precision anti-disturbance SCARA robot control method for precision assembly, characterized in that: The specific method is as follows: Use ant colony algorithm to find the working path of the robot arm; Calculate the score of each work path using the scoring function, and arrange the work paths in order of the score; The optimal working path is used as the preferred target path of the robot arm; Control the robot arm to execute according to the preferred target path; During the execution process, if a deviation occurs, the fitness function is used to determine whether to switch to another working path.

2. The high-precision anti-disturbance SCARA robot control method for precision assembly as claimed in claim 1, characterized in that: Ant colony algorithm is used to find the working path of the robot arm. The specific method is as follows: Calculate the workspace of the end effector of the robot arm, and divide the workspace into grids after obtaining it; Initialize the data of ants and the number of path nodes in the ant colony; The ant calculates the next possible path node to visit based on the pheromone concentration; Use roulette to select the next node to be visited; When all ants complete a round trip, the pheromone concentration is updated; Until the maximum number of iterations is reached, all paths are stored as working paths.

3. The high-precision anti-disturbance SCARA robot control method for precision assembly as claimed in claim 2, characterized in that: The scoring function is calculated as follows: Score=ω1·L+ω2·T+ω3·S Where L represents the total length of the path, T represents the number of turns in the path, S represents the curvature, i.e. the smoothness of the path, and ω1, ω2, and ω3 represent adjustable weights.

4. The high-precision anti-disturbance SCARA robot control method for precision assembly as claimed in claim 1, characterized in that: Control the robotic arm. The specific method is as follows: Construct a dynamic model of the robotic arm; Design adaptive robust controllers; Controlling a robotic arm with an adaptive robust controller.

5. The high-precision anti-disturbance SCARA robot control method for precision assembly as claimed in claim 4, characterized in that: Construct the dynamic model of the robotic arm. The specific method is as follows: The Lagrangian form of the dynamics of a mechanical system is as follows: Where q represents the angular displacement, represents the angular velocity, represents angular acceleration, M(q) is the inertia matrix, is the Coriolis force and centrifugal force matrix, G(q) is the gravity vector, σ(t) is the system uncertain parameter, τ(t) is the control input vector; For three-phase current decoupling of SCARA robot joint motors: Among them, u d 、u q Represent the stator voltage of d-axis and q-axis respectively, i d 、i q Represent the stator current of d-axis and q-axis respectively, R represents the stator resistance, L d , L q Represent the inductance of d-axis and q-axis respectively, ψ f represents the magnetic flux linkage, n p Represents the number of pole pairs, τ, B, J, T lp They represent the electromagnetic torque, damping coefficient, moment of inertia and load torque of the motor respectively; And because the expression of electromagnetic torque can be expressed as: In vector control, i d =0, we can get: because You can get: Rewritten into the general Lagrangian form: In the formula, the matrix F is the load torque T mentioned above lp .

6. The high-precision anti-disturbance SCARA robot control method for precision assembly as claimed in claim 5, characterized in that: Design an adaptive robust controller. The specific method is as follows: The trajectory of the working path is expressed as the first-order and second-order constraint equations: In the formula, A is the unit matrix, and matrix b is the equivalent matrix after differential transformation of matrix c; The designed adaptive robust controller includes: The nominal controller τ0 is the controller under ideal constraints; Robust controller τ1, to cope with modeling errors and initial condition deviations; Adaptive controller τ2, introduces adaptive parameters to adjust controller parameters in real time; The specific formula is as follows: in, represents the known part of the system, κ is a positive constant, the P matrix is ​​a positive definite matrix, Π(·) is a known function containing the adaptive parameter α, and the function Π(·) can be linearly decomposed; When there is no external interference and system uncertainty, That is, the controller is τ=τ0+τ1; When the initial conditions have no deviation and the model error can be ignored, that is, At this time, the controller is v=τ0.

7. The high-precision anti-disturbance SCARA robot control method for precision assembly as claimed in claim 1, characterized in that: The fitness function is used to determine whether to switch to other working paths. The specific method is as follows: The fitness function is defined as: In the formula, F current represents the current path fitness, represents the fitness of other paths; d current Indicates the Euclidean distance between the current position and the nearest point on the current target path. Indicates the deviation from the current position to other paths; Define the path switching conditions as: when When , switch to path i; where Δ is the threshold to avoid frequent switching; The controller performs trajectory tracking according to the currently selected path. After every set time, it evaluates the deviation and calculates the fitness to check whether the switching conditions are met. If the conditions are met, it updates the current path and replans the control input. Otherwise, it continues to execute along the current path.

Images