SCARA robot dynamic trajectory control method and device

Abstract

The invention discloses a dynamic trajectory control method for an SCARA (selective compliance assembly robot arm) robot. According to the kinematics model, a homogeneous transformation matrix is used for analyzing the coordinate transformation relation between each joint and the connecting rod, and an initial pose and a target pose are determined; establishing a kinetic model based on a Lagrange equation, and determining kinetic parameters needing to be identified; performing parameter identification by adopting a random weight particle swarm optimization algorithm; according to an identification result, a sliding mode robust item RBF neural network adaptive controller is constructed, real-time track control is achieved, and the end manipulator is made to move to a target pose along a preset dynamic track; a real-time pose is detected through a sensor and compared with a target pose, and arrival is judged when the error is smaller than a threshold value. Model precision and identification efficiency are improved through modeling and efficient parameter identification, the composite controller effectively compensates uncertainty, disturbance and nonlinear factors of the model, closed-loop control and online adjustment are achieved, and trajectory tracking precision, system stability and dynamic response speed are improved.

Description

Technical Field

[0001] This invention relates to a dynamic trajectory control method and apparatus for a SCARA robot. Background Technology

[0002] In modern intelligent manufacturing and automated production systems, industrial robots play an irreplaceable role as core equipment for achieving flexible, intelligent, and efficient operations. Among them, SCARA (Selective Compliance Assembly Robot Arm) robots, due to their unique mechanical structural characteristics—high compliance in the XY plane and good rigidity in the Z-axis direction—are particularly suitable for high-speed, high-precision pick-and-place, assembly, dispensing, welding, inspection, and sorting tasks, and are widely used in industries such as electronics, semiconductors, medical devices, precision instruments, and consumer electronics.

[0003] Enterprises are placing higher demands on production cycle time, positioning accuracy, operational stability, and system intelligence. Traditional trajectory control methods based on classical control theory (such as PID control) are gradually becoming insufficient to meet performance requirements under complex dynamic conditions. Especially during high-speed reciprocating motion, SCARA robots often face problems such as joint nonlinear friction, transmission backlash, load variations, external disturbances, and model uncertainties, leading to increased trajectory tracking errors, system vibration, and even instability, severely impacting production efficiency and product quality. Patent CN117484499B discloses a robust trajectory tracking SCARA robot for its robotic arm. By designing a robust controller, the robot's dynamic model is decomposed into a nominal and an uncertain part, and the uncertainties in the model are centrally incorporated into the dynamic uncertainty disturbance function, thereby effectively compensating for and eliminating the impact of system model uncertainties on control performance.

[0004] Despite substantial research advancements that have propelled the development of SCARA robot control technology, numerous challenges remain in achieving a comprehensive performance that balances high precision with excellent dynamic response. Particularly in complex trajectory tracking tasks, effectively integrating accurate dynamic models, efficient parameter identification methods, and intelligent real-time control mechanisms to construct a closed-loop control system capable of handling model uncertainties, external disturbances, and changes in operating conditions remains a key research focus and challenge. Existing methods often struggle to balance modeling accuracy, identification efficiency, and control real-time performance, lacking a systematic integrated solution. Therefore, there is an urgent need to develop a novel dynamic trajectory control method and device (system) to enhance the overall control performance of SCARA robots in high-speed, high-precision application scenarios. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and provide a dynamic trajectory control method and device for SCARA robots, so as to improve trajectory tracking accuracy and realize dynamic trajectory control of SCARA robots.

[0006] The objective of this invention is achieved through the following technical solution:

[0007] The first aspect of this invention provides a dynamic trajectory control method for a SCARA robot, the method comprising the following steps:

[0008] Based on the kinematic model of the SCARA robot, the coordinate transformation relationship of each joint and link in space is analyzed to determine the robot's initial pose and target pose; the coordinate transformation relationship is represented by a homogeneous transformation matrix.

[0009] A dynamic model of the SCARA robot is established based on the Lagrange equation to determine the dynamic parameters that need to be identified.

[0010] The dynamic parameters are identified using a stochastic weighted particle swarm optimization algorithm, and dynamic parameter identification results are generated.

[0011] Based on the identified dynamic parameters, a sliding mode robust RBF neural network adaptive controller is constructed.

[0012] The controller enables real-time control of the SCARA robot's trajectory, allowing the robot's end effector to move along a preset dynamic trajectory to the target pose.

[0013] The system detects whether the end effector has reached the target pose. If it has not, the parameters of the controller are adjusted until the end effector reaches the target pose. The detection process obtains the real-time pose information of the end effector through sensors and compares it with the target pose. When the error is less than a preset threshold, it is determined that the target pose has been reached.

[0014] This invention first establishes a kinematic model based on the robot's structural parameters, using a homogeneous transformation matrix to accurately describe the coordinate transformation relationships between each joint and link, determining the initial and target poses, and providing a geometric basis for trajectory planning. Then, a dynamic model is constructed based on the Lagrange equations, extracting key dynamic parameters, and employing a stochastic weighted particle swarm optimization (RWPSO) algorithm for high-precision identification. This effectively overcomes the problems of traditional methods easily getting trapped in local optima and slow convergence speed, obtaining dynamic parameters that closely resemble the actual system and improving model accuracy. Based on this, an adaptive controller integrating sliding mode control and an RBF neural network is fabricated. The sliding mode term enhances system robustness, while the RBF neural network approximates model uncertainties and external disturbances online and achieves adaptive compensation, significantly improving the controller's adaptability to nonlinearity and disturbances. During control execution, the controller generates real-time control commands based on the identified model, driving the robot's end effector to move along a preset dynamic trajectory. Simultaneously, sensors acquire the actual pose of the end effector in real time, performing a closed-loop comparison with the target pose. If the pose error exceeds a preset threshold, it is determined that the target has not been reached. The system dynamically adjusts the controller parameters to optimize tracking performance until the error converges to within the threshold range, completing precise positioning.

[0015] As a preferred embodiment, the step of identifying the dynamic parameters using the stochastic weighted particle swarm optimization algorithm includes:

[0016] The particle swarm is initialized, with each particle representing a possible set of dynamic parameters; the initial positions of the particle swarm are randomly initialized based on prior knowledge of the dynamic parameters to ensure the global search capability of the algorithm.

[0017] The fitness value of each particle is calculated based on the error between the output of the dynamic model and the actual measured value; the error is calculated using the root mean square error (RMSE), and its expression is as follows:

[0018]

[0019] in, For the first One actual measured value, Output values ​​for the model. The total number of samples;

[0020] Update the particle's position and velocity, where the velocity update formula is:

[0021]

[0022] The position update formula is:

[0023]

[0024] in, and The first The particle in the first Velocity and position at the next iteration For inertial weights, and As a learning factor, , A random number within the interval [0,1]. For particles The optimal position of an individual The global optimal position; the inertial weight During the iteration process, adjustments are made according to a linear decreasing strategy to balance global search capability and local search capability;

[0025] After satisfying the preset convergence conditions, the final dynamic parameter identification results are output; the convergence conditions include the rate of change of fitness value being less than a preset threshold or the number of iterations reaching a preset upper limit.

[0026] As a preferred approach, the steps for designing a sliding mode robust RBF neural network adaptive controller include:

[0027] An RBF neural network is constructed, with the robot's joint angles, velocities, and accelerations as inputs and control torque as outputs. The center and width of the RBF neural network are determined using a K-means clustering algorithm to improve the network's generalization ability.

[0028] A sliding mode control law is constructed and combined with the output of an RBF neural network to generate the final control signal. The sliding mode control law is designed using nonlinear functions to reduce jitter during the sliding mode control process.

[0029] The control signals are used to control the joints of the SCARA robot in real time, so that the robot's end effector moves along a preset dynamic trajectory.

[0030] As a preferred method, the step of detecting whether the end effector has reached the target pose includes:

[0031] The real-time pose information of the end effector is acquired; the real-time pose information is acquired through a high-precision six-dimensional force sensor and a vision sensor installed on the end effector to provide comprehensive pose data;

[0032] The error between the real-time pose and the target pose is calculated. If the error is less than a preset threshold, it is determined that the end effector has reached the target pose. The error calculation includes position error and attitude error, and different thresholds are set for each to meet the accuracy requirements of different tasks.

[0033] As a preferred embodiment, the method further includes:

[0034] After the end effector reaches the target pose, a second control command is generated based on the distance between the end effector and the target pose.

[0035] The end effector is precisely aligned with the target pose by the motors corresponding to the robot's moving joints based on the second control command; the second control command is generated by a proportional-integral-derivative (PID) controller to achieve precise position control.

[0036] As a preferred method, the steps for generating the second control command include:

[0037] Based on the distance between the end effector and the target pose and the preset ratio coefficient Calculate the control torque:

[0038]

[0039] The proportionality coefficient The control is dynamically adjusted based on the load and movement speed of the end effector to ensure stability and accuracy.

[0040] The control torque This is sent as a second control command to the motor corresponding to the moving joint.

[0041] As a preferred method, it also includes:

[0042] During the movement of the end effector along a preset dynamic trajectory, the joint torque of the robot is monitored in real time, and the parameters of the controller are adjusted according to the joint torque to compensate for external interference.

[0043] The step of real-time monitoring of the robot's joint torque includes:

[0044] The robot's joint torque data are acquired through sensors;

[0045] Based on the joint torque data and the preset compensation strategy, the parameters of the controller are adjusted to ensure that the end effector moves stably along the preset dynamic trajectory.

[0046] A second aspect of the present invention provides a dynamic trajectory control device for a SCARA robot, the device comprising:

[0047] The kinematics analysis module is used to analyze the coordinate transformation relationship of each joint and link in space based on the kinematic model of the SCARA robot, and to determine the robot's initial pose and target pose.

[0048] The dynamics modeling module is used to build a dynamics model of the SCARA robot based on the Lagrange equation and determine the dynamic parameters that need to be identified.

[0049] The parameter identification module is used to identify the dynamic parameters using a random weighted particle swarm optimization algorithm and generate dynamic parameter identification results. The parameter identification module includes a particle swarm initialization unit, a fitness calculation unit, a particle update unit, and an identification result output unit.

[0050] The controller fabrication module is used to design a sliding mode robust term RBF neural network adaptive controller based on the dynamic parameter identification results; the controller fabrication module includes a neural network construction unit, a sliding mode control law design unit, and a control signal generation unit.

[0051] The trajectory control module is used to control the trajectory of the SCARA robot in real time through the controller, so that the robot's end effector moves to the target pose along a preset dynamic trajectory; the trajectory control module includes a preset trajectory generation unit and a real-time control unit.

[0052] The detection module is used to detect whether the end effector has reached the target pose. If it has not reached the target pose, the parameters of the controller are adjusted until the end effector reaches the target pose. The detection module includes a pose acquisition unit and an error calculation unit to accurately determine whether the end effector has reached the target pose.

[0053] As a preferred embodiment, the parameter identification module further includes:

[0054] A diversity preservation unit is used to maintain the diversity of particles during particle swarm optimization to prevent the particle swarm from getting trapped in local optima. The diversity preservation unit achieves this by randomly perturbing the position and velocity of some particles. When the diversity of the particle swarm is lower than a preset threshold, the diversity preservation mechanism is automatically triggered.

[0055] The convergence speed monitoring unit is used to monitor the convergence speed of the particle swarm optimization algorithm and dynamically adjust the inertia weight and learning factor according to the convergence speed. The convergence speed monitoring unit determines the convergence speed by calculating the rate of change of fitness value in continuous iterations. When the convergence speed is lower than a preset threshold, the inertia weight and learning factor are automatically adjusted to accelerate the convergence speed.

[0056] As a preferred embodiment, the device further includes:

[0057] The pose detection module is used to acquire the real-time pose information of the end effector and calculate the error between the real-time pose and the target pose. If the error is less than a preset threshold, it is determined that the end effector has reached the target pose.

[0058] The precision alignment module is used to generate a second control command based on the distance between the end effector and the target pose after the end effector reaches the target pose, and control the end effector to precisely align with the target pose based on the second control command through the motors corresponding to the robot's moving joints;

[0059] The interference compensation module is used to monitor the joint torque, joint temperature and joint current of the robot in real time during the movement of the end effector along a preset dynamic trajectory, and adjust the parameters of the controller according to the joint torque, joint temperature and joint current to compensate for the impact of external interference and environmental changes on trajectory accuracy.

[0060] The present invention has at least the following beneficial effects:

[0061] This invention proposes a dynamic trajectory control method and device (system) for SCARA robots. By establishing an accurate kinematic model based on a homogeneous transformation matrix and a dynamic model based on the Lagrange equation, the robot's pose transformation relationship and dynamic characteristics are accurately described. Furthermore, by combining a stochastic weighted particle swarm optimization algorithm, key dynamic parameters are efficiently identified. This effectively overcomes the problems of inaccurate parameter acquisition and easy getting trapped in local optima in traditional methods, significantly improving model accuracy and identification efficiency, and laying a reliable theoretical foundation for high-precision control.

[0062] Based on this, the composite controller can compensate for the effects of model uncertainties, external disturbances, and nonlinear factors in real time, and achieve closed-loop control and online parameter adjustment through sensor feedback, ensuring that the end effector accurately tracks the preset trajectory and stably reaches the target pose. This invention not only significantly improves trajectory tracking accuracy, system stability, and dynamic response speed, but also effectively suppresses chattering, making it suitable for high-speed and high-precision industrial applications. Attached Figure Description

[0063] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show embodiments of the present invention and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained from these drawings without creative effort.

[0064] Figure 1 This is a schematic diagram of a dynamic trajectory control method for a SCARA robot. Detailed Implementation

[0065] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings, but the scope of protection of the present invention is not limited to the following description.

[0066] In the following description, different embodiments of the present disclosure will be described with reference to the accompanying drawings. However, it should be understood that the present disclosure is not intended to be limited to the specific forms disclosed herein, but rather should be interpreted as covering various changes, equivalents, and / or substitutions of the embodiments of the present disclosure. In describing the drawings, similar reference numerals may be used to indicate similar constituent elements.

[0067] The expressions used in the different embodiments of this disclosure, such as "first," "second," "the first," or "the second," may modify different elements regardless of their order and / or importance, and do not limit the corresponding elements. For example, a first user device and a second user device refer to different user devices, although both are user devices. For example, a first element may be referred to as a second element, and similarly, a second element may be referred to as a first element, without departing from the scope of this disclosure.

[0068] It should be understood that specific details are provided in the following description to facilitate a complete understanding of the exemplary embodiments. However, those skilled in the art will understand that the exemplary embodiments can be implemented without these specific details. For example, the system may be shown in block diagrams to avoid obscuring the example with unnecessary details. In other instances, well-known processes, structures, and techniques may be omitted with non-essential details to avoid obscuring the example.

[0069] like Figure 1 As shown, a dynamic trajectory control method for a SCARA robot includes the following steps:

[0070] Based on the kinematic model of the SCARA robot, the coordinate transformation relationship of each joint and link in space is analyzed to determine the robot's initial pose and target pose. The coordinate transformation relationship is represented by a homogeneous transformation matrix, and an error compensation mechanism is introduced during the calculation process to improve the accuracy of pose determination.

[0071] A dynamic model of the SCARA robot is established based on the Lagrange equation to determine the dynamic parameters that need to be identified. The dynamic model considers various factors such as the friction force, inertial force and external disturbance force of the robot joints to more accurately describe the dynamic behavior of the robot.

[0072] The dynamic parameters are identified using a stochastic weighted particle swarm optimization algorithm, generating dynamic parameter identification results. The stochastic weighted particle swarm optimization algorithm dynamically adjusts the inertia weight and learning factor during the iteration process to accelerate the convergence speed and improve the identification accuracy.

[0073] Based on the identified dynamic parameters, a sliding mode robust RBF neural network adaptive controller is fabricated. The controller combines the speed of sliding mode control with the adaptive capability of the RBF neural network, and can effectively cope with changes in system parameters and external disturbances.

[0074] The controller performs real-time control of the SCARA robot's trajectory, enabling the robot's end effector to move to the target pose along a preset dynamic trajectory. The preset dynamic trajectory is generated according to task requirements using a fifth-order polynomial interpolation method to ensure the smoothness and continuity of the trajectory.

[0075] The system detects whether the end effector has reached the target pose. If it has not, the parameters of the controller are adjusted (the error between the real-time pose of the end effector and the target pose is calculated, and the parameters of the controller are adjusted according to the error. The adjusted controller then re-controls the trajectory of the SCARA robot in real time, so that the robot's end effector continues to move along the preset dynamic trajectory) until the end effector reaches the target pose. The detection process obtains the real-time pose information of the end effector through a vision sensor and compares it with the target pose. When the error is less than a preset threshold, it is determined that the target pose has been reached.

[0076] In one embodiment, firstly, a kinematic model is established based on the Denavit-Hartenberg (DH) parameters of the SCARA robot, and the spatial geometric relationships between each joint and link are analyzed. The coordinate transformation process from the base to the end effector is calculated step-by-step using a homogeneous transformation matrix, ultimately obtaining the position and orientation of the end effector in Cartesian space. To improve the accuracy of pose calculation, an error compensation mechanism is introduced during the forward kinematics solution process. This mechanism constructs an error Jacobian matrix or uses a neural network to fit the error mapping relationship based on pre-calibrated structural parameter deviations (such as link length errors, joint offset angle errors, assembly tilt, etc.), and corrects the theoretical pose online. For example, the actual poses of multiple calibration points are collected using a laser tracker or vision system, compared with theoretical values, and the geometric error parameters of each link are identified using the least squares method. These errors are then compensated in real time during subsequent kinematic calculations, significantly improving the accuracy of determining the initial and target poses.

[0077] In one example, the SCARA robot's structure includes four degrees of freedom: two rotary joints J1 and J2 for planar motion in the horizontal plane, a third joint J3 controlling the rotation angle of the end effector, and a fourth joint J4, a linear lifting joint, controlling the end effector's height. The lengths of each link are as follows: Rice (first arm) Meters (second arm) Meters (vertical offset from the end of the second arm to the rotation joint).

[0078] In the initial state, the angles of each joint are: radian, radian, radian, Meters. A kinematic model is established based on the DH parameter method, and the coordinate transformation relationships are calculated step-by-step using the homogeneous transformation matrix. Based on the DH parameter table, the homogeneous transformation matrix for each joint can be constructed, and the homogeneous transformation matrix of the end effector is obtained through matrix multiplication. The first arm extends fully along the x-axis, and the second arm is collinear with it; therefore, the position of the end effector in the xy-plane is... rice, meters; z direction from and Joint decision, that is Meters. The end-effector attitude is controlled by J3, initially... Corresponding to rotation around the z-axis The pose matrix is ​​the identity matrix. The complete initial pose homogeneous transformation matrix is:

[0079]

[0080] The target pose is set as follows: the end effector needs to move to... meters, and rotate about the z-axis ( (Radians) to adjust the grasping direction. The target pose matrix is:

[0081]

[0082] To achieve this target pose, inverse kinematics solutions are required to calculate the corresponding target joint variables. The planar position can then be solved using analytical or numerical iterative methods. corresponding and Calculations show that the point is located within the workspace, and there are two solutions (elbow upward / downward). One feasible solution is selected: radians (approximately) ), radians (approximately) The end-effector rotation angle is determined by the task requirements. Radius. The target value for the lifting joint is... rice.

[0083] The joint variables mentioned above were substituted into the forward kinematics model (which was established based on the DH parameter method) for verification: The calculation yielded... rice, rice, The meter, the end attitude is a rotation about the z-axis. The pose is consistent with the target pose, and the verification is successful.

[0084] By establishing a kinematic model of the SCARA robot, the initial pose and target pose are accurately described using the homogeneous transformation matrix, and the target joint variables are obtained by combining inverse kinematics solution. This ensures that the end effector can move accurately from the starting position to the target position and complete the attitude adjustment, providing a foundation for subsequent trajectory planning and high-precision control.

[0085] Next, a complete dynamic model of the SCARA robot was established based on the Lagrange equations. This model constructs a Lagrange function using the difference between the system's kinetic and potential energy. By differentiating the generalized coordinates (angles of each joint), dynamic equations including the inertia matrix, Coriolis / centripetal force terms, gravity terms, and friction terms were derived. Friction was described using the Stribeck friction model, which includes static friction, Coulomb friction, and viscous friction components; external disturbances were included as unknown bounded disturbance terms in the model. This refined modeling approach more realistically reflects the robot's dynamic behavior during startup, low-speed operation, and variable load conditions, providing a reliable foundation for subsequent high-precision control.

[0086] To obtain accurate dynamic parameters (such as the mass of each link, center of mass coordinates, moment of inertia, and friction coefficient), a stochastic weighted particle swarm optimization (RWPSO) algorithm is used for parameter identification. Specifically, an excitation trajectory (such as a multi-frequency superimposed sinusoidal signal) is first designed to drive the robot and simultaneously collect joint angles, angular velocities, angular accelerations, and input torque data. Using the sum of squared errors between the measured torque and the model-predicted torque as the optimization objective function, the particle swarm algorithm searches for the optimal parameter combination in the solution space. During the iteration process, the inertia weight adopts a linear decreasing strategy (e.g., decreasing from 0.9 to 0.4), while the learning factor is dynamically adjusted according to population diversity to balance global exploration and local exploitation capabilities, accelerate convergence speed, and avoid premature convergence, ultimately outputting high-precision dynamic parameter identification results.

[0087] The Lagrange equations are used to establish the dynamic model of the SCARA robot, providing a theoretical framework for describing the system's motion and force relationships. Meanwhile, the Randomized Weighted Particle Swarm Optimization (RWPSO) algorithm is used to identify unknown dynamic parameters in the model (such as mass, moment of inertia, and friction coefficient), optimizing the model output to approximate actual measured data. Together, the former determines the model structure, and the latter determines the model parameters, constructing an accurate and usable robot dynamic model, laying the foundation for high-performance control.

[0088] Based on the identified accurate dynamic model, a sliding mode robust RBF neural network adaptive controller (an adaptive controller based on sliding mode control and RBF neural network) is designed. The controller structure consists of two parts: a nominal control term, which generates the basic control input based on feedback linearization or the torque computation method; and a compensation term, which is composed of an RBF neural network. Its input is the tracking error and its derivative, and the network output is used to approximate the unmodeled dynamics, parameter deviations, and external disturbances. Simultaneously, a sliding mode robust term (such as a sign function or a saturation function) is introduced into the control law to enhance the system's ability to suppress uncertainties. The RBF network weights are designed using Lyapunov stability theory to design an adaptive law that updates online, ensuring the global asymptotic stability of the closed-loop system. This composite control strategy combines fast response, strong robustness, and self-learning capability, effectively addressing nonlinearity and disturbance problems in high-speed motion.

[0089] During the trajectory planning phase, a preset dynamic trajectory is generated using a fifth-order polynomial interpolation method based on task requirements (such as start point, end point, and intermediate path points). Position, velocity, and acceleration are set to zero (or specified values) at both the start and end points. The coefficients of the fifth-order polynomial are determined by solving a system of linear equations, ensuring the trajectory is continuous and smooth in the time domain. This trajectory not only meets kinematic continuity requirements but also effectively suppresses vibration and impact, making it suitable for scenarios with high smoothness requirements, such as high-speed dispensing and assembly.

[0090] The SCARA robot is controlled in real time via the controller described above. The control cycle is typically set to 1-2ms. Based on the angle and speed information fed back by the current joint encoders, the controller combines the disturbance estimated by the identification model and RBF network to calculate the required joint torque in real time and sends it to the servo driver for execution, driving the end effector to move along a fifth-order polynomial trajectory.

[0091] During the movement, a high-precision vision sensor (such as an industrial camera in conjunction with marker points) is used to detect the actual pose of the end effector in real time. Image processing algorithms (such as template matching or edge detection) extract the coordinates of the marker center and, combined with camera calibration parameters, convert them into a three-dimensional position and angle in the world coordinate system. This real-time pose is then compared point-by-point with the target trajectory to calculate the position error and attitude error.

[0092] If the current error is greater than the preset threshold (e.g., position error > 0.1mm, angle error > 0.1mm), then the error is greater than the preset threshold. If the target pose has not yet been reached, the system initiates a parameter adjustment mechanism: on the one hand, the RBF neural network continues to learn error characteristics online and updates weights to enhance disturbance compensation capabilities; on the other hand, the sliding mode gain in the controller can be adaptively adjusted according to the error magnitude (e.g., using fuzzy rules or exponential functions) to avoid excessive chattering or slow response. The adjusted controller regenerates control commands, realizing a closed-loop optimization process of detection, adjustment, and re-control.

[0093] The system only determines that the end effector has accurately reached the target pose and ends the current motion task when the error between the real-time pose obtained from visual feedback and the target pose remains below a preset threshold. This closed-loop mechanism ensures millimeter-level positioning accuracy even in the presence of external disturbances or model deviations.

[0094] In a preferred embodiment, the step of identifying the dynamic parameters using a stochastic weighted particle swarm optimization algorithm includes:

[0095] The particle swarm is initialized, with each particle representing a set of possible dynamic parameters (including moment of inertia, link mass, link length, friction coefficient, center of mass position, and joint offset). The initial positions of the particle swarm are randomly initialized based on prior knowledge of the dynamic parameters to ensure the algorithm's global search capability. During particle swarm initialization, the initial values ​​of these parameters are typically randomly generated based on prior knowledge. For example, if the mass of the first link is known to be approximately 1 kg, the initial value of m1 can be randomly generated between 0.8 kg and 1.2 kg. In this way, the particle swarm optimization algorithm can perform a broad search across the entire possible solution space to find the optimal set of dynamic parameters.

[0096] The fitness value of each particle is calculated based on the error between the output of the dynamics model and the actual measured value. This error is calculated using the root mean square error (RMSE), which can be used as the particle's fitness value to evaluate the performance of the set of dynamic parameters represented by the particle. Its expression is as follows:

[0097] in, For the first One actual measured value, Output values ​​for the model. The total number of samples;

[0098] Update the particle's position and velocity, where the velocity update formula is:

[0099]

[0100] The position update formula is:

[0101]

[0102] in, and The first The particle in the first Velocity and position at the next iteration For inertial weights, and As a learning factor, A random number within the interval [0,1]. For particles The optimal position of an individual The globally optimal position; For the first The particle in the first Speed ​​during the next iteration; Indicates the first The particle in the first The position at the next iteration. The inertia weight. During the iteration process, adjustments are made according to a linear decreasing strategy to balance global search capability and local search capability.

[0103] The final dynamic parameter identification result is output until the preset convergence condition is met; the convergence condition includes the rate of change of fitness value being less than a preset threshold or the number of iterations reaching a preset upper limit.

[0104] The process of identifying dynamic parameters using the Randomized Weighted Particle Swarm Optimization (RWPSO) algorithm is as follows: First, the particle swarm needs to be initialized, with each particle representing a possible set of dynamic parameters. The initial positions of these particles are randomly determined based on prior knowledge of the dynamic parameters. This is done to allow the algorithm to perform a broad search across the entire possible solution space, thus ensuring good global search capabilities. Next, the fitness value of each particle is calculated. The fitness value is determined by comparing the error between the output of the dynamic model and the actual measured value. Here, the root mean square error (RMSE) is used to quantify this error. The smaller the RMSE, the closer the model output is to the actual measured value, and the higher the particle's fitness. Then, the particle's position and velocity are updated. The velocity update considers three factors: the particle's current velocity, its own historical best position, and its global best position. Among them, the inertia weight controls the degree to which the particle maintains its original motion trend, and the learning factor (… and These respectively adjust the tendency of the particle to move towards its historical best position and its global best position. Random numbers ( The introduction of ) adds a degree of randomness to the particle's motion, helping to prevent the particle from getting trapped in local optima. The position update is based on the updated velocity to adjust the particle's position in the solution space.

[0105] The inertia weight is adjusted during iteration according to a linear decreasing strategy. Initially, a larger weight... The value allows the particle to explore extensively in the solution space, as iterations proceed. As the fitness value gradually decreases, the particle's motion gradually stabilizes, allowing for a more detailed search near the global optimum. This helps balance global and local search capabilities. This process continues until a preset convergence condition is met. The convergence condition can be that the rate of change of the fitness value is less than a preset threshold, or that the number of iterations reaches a preset upper limit. When one of these conditions is met, the algorithm stops iterating and outputs the final dynamic parameter identification results. In this way, the stochastic weighted particle swarm optimization algorithm can effectively search for optimal dynamic parameters in a complex solution space, thereby improving the accuracy and reliability of the dynamic model.

[0106] In a preferred embodiment, the steps of designing a sliding mode robust RBF neural network adaptive controller include:

[0107] An RBF neural network is constructed, with the robot's joint angles, velocities, and accelerations as inputs and control torque as outputs. The center and width of the RBF neural network are determined using a K-means clustering algorithm to improve the network's generalization ability.

[0108] A sliding mode control law is constructed and combined with the output of an RBF neural network to generate the final control signal. The sliding mode control law is designed using nonlinear functions to reduce jitter during the sliding mode control process.

[0109] The control signals are used to control the joints of the SCARA robot in real time, so that the robot's end effector moves along a preset dynamic trajectory. The control signals are calculated and output in real time by a digital signal processor (DSP) to ensure the real-time performance and accuracy of the control.

[0110] The process of designing a sliding mode robust RBF neural network adaptive controller can be divided into several key steps. First, an RBF (Radial Basis Function) neural network needs to be constructed. The inputs to this network are the robot's joint angles, velocities, and accelerations; these input data reflect the robot's current motion state. The output of the network is the control torque, which will be used to adjust the robot's motion so that it can operate according to a preset trajectory.

[0111] To improve the generalization ability of the RBF neural network, i.e., its adaptability to unseen data, the network's center and width parameters are determined using the K-means clustering algorithm. K-means clustering automatically finds cluster centers based on the distribution of the input data; these centers are used as the center parameters of the RBF neural network, while the width parameter is set according to the distribution range of the clusters. Parameters determined in this way allow the neural network to better capture the features of the input data, thereby improving its adaptability to different motion states.

[0112] Designing a sliding mode control law. Sliding mode control is a special variable structure control method that allows the system state to quickly reach and remain on a preset sliding surface by switching between different control modes. In this process, the output of the RBF neural network is incorporated as part of the sliding mode control law to generate the final control signal. This control signal not only incorporates the fast response characteristics of sliding mode control but also compensates for unmodeled dynamics, parameter deviations, and external disturbances through the adaptive capability of the neural network, thereby improving the reliability of the control.

[0113] To reduce potential jitter during sliding mode control, the sliding mode control law is designed using a nonlinear function (such as the SAT saturation function). The nonlinear function ensures rapid system state transitions while smoothly transitioning control signals, avoiding system jitter caused by drastic changes in the control signal. This design improves system stability and reliability while maintaining control accuracy.

[0114] The joints of the SCARA robot are controlled in real time using the aforementioned control signals. The calculation and output of the control signals are performed by a digital signal processor (DSP). The DSP has the ability to process data at high speed, ensuring the real-time performance and accuracy of the control signals. This means that the robot can quickly adjust the movement of its joints according to its real-time motion state, enabling the end effector to move precisely along a preset dynamic trajectory and complete various complex tasks.

[0115] In summary, this controller design combines the adaptive capabilities of RBF neural networks with the fast response characteristics of sliding mode control. It optimizes neural network parameters using K-means clustering and employs nonlinear functions to design sliding mode control laws to reduce jitter. Finally, precise real-time control is achieved through a DSP. This design approach enables the controller to not only adapt to the robot's dynamic changes but also maintain good control performance in the face of external disturbances and parameter uncertainties, providing strong support for high-precision motion control of robots.

[0116] In a preferred embodiment, the step of detecting whether the end effector has reached the target pose includes:

[0117] The real-time pose information of the end effector is acquired; the real-time pose information is acquired through a high-precision six-dimensional force sensor and a vision sensor installed on the end effector to provide comprehensive pose data;

[0118] The error between the real-time pose and the target pose is calculated. If the error is less than a preset threshold, it is determined that the end effector has reached the target pose. The error calculation includes position error and attitude error, and different thresholds are set for each to meet the accuracy requirements of different tasks.

[0119] Detecting whether the end effector has reached the target pose is crucial for ensuring the robot's accurate task execution. This process first acquires real-time pose information using a high-precision six-dimensional force sensor and a vision sensor mounted on the end effector: the six-dimensional force sensor measures force and torque in three directions, while the vision sensor provides spatial position and orientation data. The fusion of these two sensors provides comprehensive and accurate feedback to the control system. The system calculates the error between the real-time pose and the target pose, including position and orientation errors, and sets corresponding thresholds based on specific task requirements. When the error is less than the preset threshold, the end effector is considered to have accurately reached the target pose, and the task is completed. If the error exceeds the threshold, the control system adjusts the control signal according to the error magnitude, continuously guiding the end effector closer to the target until the accuracy requirements are met. This method, combining high-precision sensing with a flexible error judgment mechanism, not only improves the accuracy and reliability of robot operation but also enhances its adaptability to perform diverse tasks in complex and changing environments.

[0120] In a preferred embodiment, the method further includes:

[0121] After the end effector reaches the target pose, a second control command is generated based on the distance between the end effector and the target pose; the distance is measured by a laser rangefinder on the end effector to provide high-precision distance data.

[0122] The end effector is precisely aligned with the target pose by the motors corresponding to the robot's moving joints based on the second control command; the second control command is generated by a proportional-integral-derivative (PID) controller to achieve precise position control.

[0123] In a preferred embodiment, the step of generating the second control command includes:

[0124] Based on the distance between the end effector and the target pose and the preset ratio coefficient Calculate the control torque:

[0125]

[0126] Among them, distance The proportionality coefficient is in meters (m). Control torque in Newtons per meter (N / m) The unit is Newton-meter (N·m). The proportionality coefficient is... The control is dynamically adjusted based on the load and movement speed of the end effector to ensure stability and accuracy.

[0127] The control torque The second control command is sent to the motor corresponding to the moving joint; the second control command is sent through a high-speed communication interface to ensure real-time transmission of the command.

[0128] As the robot's end effector approaches the target pose, the system generates a second control command for fine-tuning to ensure precise alignment. This process begins by using a laser rangefinder on the end effector to accurately measure the distance between it and the target pose. This allows for the acquisition of high-precision proximity information. Subsequently, based on the measured distance... and the proportional coefficient of dynamic adjustment Calculate control torque The proportionality coefficient The proportional coefficient is adjusted in real time based on the end load and motion speed to balance control stability and response accuracy under different operating conditions. The dynamic adjustment mechanism aims to optimize control performance in real time based on the actual load and speed of the end effector. When the load is heavy or the speed is high, the adjustment needs to be increased. To improve the system's responsiveness and driving force, ensuring rapid and stable approach to the target pose; while when the load is light or the speed is low, the force should be appropriately reduced. To avoid oscillations caused by over-adjustment and improve control stability.

[0129] Calculated control torque As the second control command, it is generated by the PID controller and sent to the drive motor of the moving joint. The PID controller provides feedback on the distance error. Continuous output adjustment enables high-precision closed-loop position control. Commands are transmitted in real-time via a high-speed communication interface, ensuring low-latency execution of control signals and thus enabling rapid and accurate final position fine-tuning of the end effector. This precise alignment mechanism is crucial in scenarios with extremely high precision requirements, such as industrial assembly and medical surgery. Through dynamic parameter adjustment and high-speed response control, it significantly improves the robot's operational accuracy and adaptability in complex tasks.

[0130] In a preferred embodiment, it further includes:

[0131] During the movement of the end effector along a preset dynamic trajectory, the joint torque of the robot is monitored in real time, and the parameters of the controller are adjusted according to the joint torque to compensate for external interference; the joint torque is monitored in real time by a high-precision torque sensor to provide accurate torque data;

[0132] The step of real-time monitoring of the robot's joint torque includes:

[0133] The robot acquires joint torque data through sensors, including torque sensors mounted on the joint motors and six-dimensional force sensors mounted on the end effector, to provide comprehensive torque data.

[0134] Based on the joint torque data and the preset compensation strategy, the parameters of the controller are adjusted to ensure that the end effector moves stably along the preset dynamic trajectory; the compensation strategy is implemented by a fuzzy logic controller, which dynamically adjusts the parameters of the controller according to the magnitude and trend of the torque data.

[0135] During the robot's end effector's movement along a preset trajectory, real-time monitoring of joint torques is necessary to ensure stability and accuracy. This process is accomplished collaboratively by high-precision torque sensors mounted on the joint motors and a six-dimensional force sensor on the end effector: the former directly measures the output torque of each joint, reflecting the internal driving force state, while the latter acquires force and torque information in three directions at the end effector to sense external environmental forces. By fusing data from both types of sensors, a comprehensive understanding of the internal driving and external disturbance torques experienced by the robot during its movement can be achieved. Based on this real-time torque information, the system employs a fuzzy logic controller to implement a dynamic compensation strategy. This controller, without requiring a precise mathematical model, can determine the degree of disturbance based on the magnitude and trend of torque changes and intelligently adjust the controller parameters.

[0136] Fuzzy logic controllers achieve intelligent dynamic adjustment of robot control parameters by simulating human experience-based decision-making. Their workflow begins by using real-time monitored joint torques and their rate of change as input variables, and adjustments to parameters such as control gain as output variables. Then, through fuzzification, the precise torque data is mapped to fuzzy sets such as "small, medium, large" or "slow, medium, fast." This is combined with fuzzy inference based on expert experience-based "IF-THEN" rules (e.g., "if the torque is large and changes rapidly, increase the control gain") to derive the fuzzy state of the output variables. Finally, defuzzification (e.g., using the center-of-gravity method) converts these values ​​into specific parameter adjustments for real-time controller gain correction. This process constitutes a closed-loop feedback system that continuously optimizes the control strategy dynamically based on external disturbances. It enhances response during periods of high disturbance and avoids overshoot during stable operation, effectively compensating for nonlinear disturbances and ensuring the end effector can move stably and accurately along a preset trajectory even under complex conditions, significantly improving the robot system's adaptability. When a significant external disturbance is detected, the control gain is automatically increased to enhance anti-interference capability and response speed; when the system is running smoothly, the gain is maintained or appropriately reduced to avoid over-adjustment that could cause oscillations. Through this closed-loop real-time monitoring and adaptive compensation mechanism, the robot can effectively suppress external disturbances, ensuring that the end effector accurately and stably tracks the dynamic trajectory, significantly improving the system's adaptability under complex working conditions.

[0137] In a preferred embodiment, it further includes:

[0138] During the movement of the end effector along a preset dynamic trajectory, the joint temperature of the robot is monitored in real time, and the parameters of the controller are adjusted according to the joint temperature to compensate for the influence of thermal expansion on the trajectory accuracy; the joint temperature is monitored in real time by thermocouple sensors installed at the joints to provide accurate temperature data.

[0139] The step of real-time monitoring of the robot's joint temperature includes:

[0140] The robot acquires joint temperature data through sensors, including thermocouple sensors mounted on the joint motors and thermistor sensors mounted on the end effector, to provide comprehensive temperature data.

[0141] Based on the joint temperature data and a preset compensation strategy, the parameters of the controller are adjusted to ensure that the end effector moves stably along a preset dynamic trajectory. The compensation strategy is implemented through a temperature compensation algorithm, which dynamically adjusts the parameters of the controller according to the magnitude and trend of the temperature data.

[0142] During the movement of the robot's end effector along a preset trajectory, changes in joint temperature can cause thermal expansion of components, leading to deformation of the mechanical structure and affecting the accuracy of the end effector trajectory. To address this issue, the system uses thermocouple sensors mounted on the joint motors and thermistor sensors on the end effector to collect temperature data from key components in real time. These two types of sensors complement each other, ensuring comprehensive and accurate temperature monitoring. After acquiring temperature information, the system combines a preset temperature compensation algorithm to dynamically adjust controller parameters based on the temperature level and its changing trends. For example, when the joint temperature rises and the risk of thermal expansion increases, the algorithm appropriately reduces the control gain to avoid overshoot or oscillation caused by structural deformation; conversely, when the temperature drops, the gain is moderately increased to maintain the system's response speed and control accuracy. This compensation mechanism proactively adjusts the system in the early stages of temperature changes, without waiting for significant trajectory deviations to appear, effectively suppressing the effects of thermal deformation. The entire process operates in a real-time closed-loop manner, enabling the robot to maintain good trajectory stability during long-term or variable-condition operations, significantly improving the system's reliability and adaptability in precision operations.

[0143] In a preferred embodiment, the method further includes: monitoring the joint current of the robot in real time during the movement of the end effector along a preset dynamic trajectory, and adjusting the parameters of the controller according to the joint current to compensate for the impact of motor load changes on trajectory accuracy; the joint current is monitored in real time by a current sensor installed on the joint motor to provide accurate current data.

[0144] The step of real-time monitoring of the robot's joint current includes:

[0145] The robot's joint current data is acquired through current sensors; the current sensors are installed in the drive circuit of each joint motor and can measure the motor's current value in real time.

[0146] Based on the joint current data and the preset compensation strategy, the parameters of the controller are adjusted to ensure that the end effector moves stably along the preset dynamic trajectory. The compensation strategy is implemented through an adaptive control algorithm, which dynamically adjusts the parameters of the controller according to the magnitude and trend of the current data to compensate for the impact of motor load changes on trajectory accuracy.

[0147] During the robot's end effector's movement along a preset dynamic trajectory, changes in joint current reflect fluctuations in motor load in real time. Load changes can cause motion lag or trajectory deviation, affecting operational accuracy. To address this, the system uses high-precision current sensors installed in the drive circuits of each joint motor to collect joint current data in real time. These sensors possess excellent responsiveness and measurement accuracy, ensuring the control system can detect load increases or decreases immediately. After acquiring current information, the system implements a dynamic compensation strategy using an adaptive control algorithm: when an increase in current is detected, indicating a heavier motor load, the system automatically increases the control gain to enhance driving force and maintain trajectory tracking capability; when the current decreases and the load lightens, the gain is appropriately reduced to avoid vibration or overshoot due to excessive response. The entire adjustment process is continuous and real-time, requiring no precise mathematical model, but rather autonomously optimizing control parameters based on actual working conditions. Through this current feedback-based adaptive adjustment mechanism, the robot can maintain motion stability and trajectory accuracy under complex working conditions with dynamically changing loads, improving system adaptability and task execution reliability.

[0148] A dynamic trajectory control device or system for a SCARA robot, the device or system comprising:

[0149] The kinematic analysis module is used to analyze the coordinate transformation relationship of each joint and link in space based on the kinematic model of the SCARA robot, and to determine the initial pose and target pose of the robot. The kinematic analysis module includes a homogeneous transformation matrix calculation unit and an error compensation unit to improve the accuracy of pose determination.

[0150] The dynamic modeling module is used to establish a dynamic model of the SCARA robot based on the Lagrange equation and determine the dynamic parameters that need to be identified. The dynamic modeling module considers various factors such as the friction force, inertial force and external disturbance force of the robot joints to more accurately describe the dynamic behavior of the robot.

[0151] The parameter identification module is used to identify the dynamic parameters using a random weighted particle swarm optimization algorithm and generate dynamic parameter identification results. The parameter identification module includes a particle swarm initialization unit, a fitness calculation unit, a particle update unit, and an identification result output unit to achieve efficient dynamic parameter identification.

[0152] The controller fabrication module is used to design a sliding mode robust term RBF neural network adaptive controller based on the dynamic parameter identification results. The controller fabrication module includes a neural network construction unit, a sliding mode control law design unit, and a control signal generation unit to achieve precise trajectory control.

[0153] The trajectory control module is used to control the trajectory of the SCARA robot in real time through the controller, so that the robot's end effector moves to the target pose along a preset dynamic trajectory; the trajectory control module includes a preset trajectory generation unit and a real-time control unit to ensure the smoothness and continuity of trajectory control.

[0154] The detection module is used to detect whether the end effector has reached the target pose. If it has not reached the target pose, the parameters of the controller are adjusted and the above control steps are repeated until the end effector reaches the target pose. The detection module includes a pose acquisition unit and an error calculation unit to accurately determine whether the end effector has reached the target pose.

[0155] This dynamic trajectory control device for SCARA robots comprehensively improves the accuracy and stability of the end effector in preset trajectory motion through the collaborative work of multiple functional modules. The system first uses a kinematic analysis module to accurately calculate the position and attitude relationships between joints and links based on the robot's kinematic model, utilizing a homogeneous transformation matrix to determine the initial and target poses, and improves the accuracy of pose calculation through an error compensation mechanism. Subsequently, a dynamic modeling module constructs a complete dynamic model including friction, inertial forces, and external disturbances based on the Lagrange equations, providing a theoretical basis for parameter identification. The parameter identification module employs an improved stochastic weighted particle swarm optimization algorithm, efficiently identifying key dynamic parameters through particle initialization, fitness evaluation, and iterative updates. Based on these accurate parameters, a controller fabrication module designs an adaptive controller integrating sliding mode control and an RBF neural network, effectively handling system uncertainties and external disturbances. The trajectory control module then utilizes this controller, combined with preset trajectory generation and real-time feedback control, to ensure the end effector moves smoothly and continuously along the dynamic trajectory. The detection module uses pose acquisition and error calculation to determine in real time whether the end effector has reached the target pose. If it fails to meet the target, it triggers the controller parameters to adjust and repeatedly executes the control process until precise positioning is achieved. Through the close collaboration of its modules, the entire system significantly improves the trajectory tracking accuracy, system stability, and environmental adaptability of SCARA robots in complex work scenarios.

[0156] In a preferred embodiment, the parameter identification module further includes:

[0157] A diversity preservation unit is used to maintain the diversity of particles during particle swarm optimization to prevent the particle swarm from getting trapped in local optima. The diversity preservation unit achieves this by randomly perturbing the position and velocity of some particles. When the diversity of the particle swarm is lower than a preset threshold, the diversity preservation mechanism is automatically triggered.

[0158] The convergence speed monitoring unit is used to monitor the convergence speed of the particle swarm optimization algorithm and dynamically adjust the inertia weight and learning factor according to the convergence speed. The convergence speed monitoring unit determines the convergence speed by calculating the rate of change of fitness value in continuous iterations. When the convergence speed is lower than a preset threshold, the inertia weight and learning factor are automatically adjusted to accelerate the convergence speed.

[0159] In the process of identifying the dynamic parameters of the SCARA robot, the Particle Swarm Optimization (PSO) algorithm significantly improves optimization performance and solution efficiency by introducing a diversity preservation unit and a convergence rate monitoring unit. The diversity preservation unit prevents the algorithm from getting trapped in local optima prematurely. Its working principle is to automatically trigger a random perturbation mechanism when the particle swarm distribution is too concentrated and the diversity is below a preset threshold. This applies small random changes to the position and velocity of some particles, thereby pushing them out of local extreme regions, enhancing their exploration ability in the solution space, and increasing the probability of finding the global optimum. Simultaneously, the convergence rate monitoring unit monitors the convergence status of the algorithm in real time, judging the convergence speed by calculating the rate of change of fitness values ​​in continuous iterations. When the convergence speed is found to be too slow, this unit dynamically adjusts the key parameters of PSO: appropriately reducing the inertia weight to reduce the influence of particle inertial motion and enhance the convergence trend towards individual and global optima; and moderately increasing the learning factor to accelerate the tracking speed of particles towards the optimal solution. These two units work together to ensure the breadth of the search through a diversity preservation mechanism and improve convergence efficiency through dynamic parameter adjustment. This enables the PSO algorithm to have both global exploration capabilities and fast convergence characteristics during parameter identification, thereby obtaining high-precision dynamic parameters more efficiently and accurately, providing a reliable foundation for the design of subsequent high-performance controllers.

[0160] In a preferred embodiment, the device further includes:

[0161] The pose detection module is used to acquire the real-time pose information of the end effector and calculate the error between the real-time pose and the target pose. If the error is less than a preset threshold, it is determined that the end effector has reached the target pose. The pose detection module includes a visual sensor, a six-dimensional force sensor and an error calculation unit to provide comprehensive pose data and accurate error judgment.

[0162] A precision alignment module is used to generate a second control command based on the distance between the end effector and the target pose after the end effector reaches the target pose. The second control command is then used to control the end effector to precisely align with the target pose via the motors corresponding to the robot's moving joints. The precision alignment module includes a laser rangefinder, a PID controller, and a second control command generation unit to achieve precise alignment of the end effector.

[0163] The interference compensation module is used to monitor the joint torque, joint temperature, and joint current of the robot in real time during the movement of the end effector along a preset dynamic trajectory, and adjust the parameters of the controller according to the joint torque, joint temperature, and joint current to compensate for the impact of external interference and environmental changes on trajectory accuracy. The interference compensation module includes a torque sensor, a thermocouple sensor, a current sensor, and an adaptive compensation unit to achieve real-time compensation for various interference factors.

[0164] The device utilizes multiple functional modules working in concert to ensure that the SCARA robot's end effector can accurately reach and stably maintain the target pose. First, the pose detection module uses visual sensors and a six-dimensional force sensor to acquire the end effector's position and attitude information in real time. The error calculation unit compares this data with the preset target pose; when the error is less than a set threshold, the system determines that the end effector has accurately reached its position. During the approach to the target pose, the precision alignment module is activated, using a laser rangefinder to accurately measure the distance between the end effector and the target. A PID controller then generates a second control command, which is sent to the motors of each moving joint to achieve fine-tuning of the position, thus ensuring accurate positioning in high-precision tasks. Simultaneously, the interference compensation module continuously operates throughout the entire movement process, using torque sensors, thermocouple sensors, and current sensors to monitor joint torque, temperature, and current changes in real time, comprehensively sensing external interference and operational fluctuations. The adaptive compensation unit dynamically adjusts the controller parameters based on this data, effectively suppressing trajectory deviations caused by factors such as thermal expansion and load changes. Through precise pose detection, fine-tuning of accurate alignment, and real-time optimization of interference compensation, the device achieves closed-loop control throughout the entire process, significantly improving the robot's trajectory accuracy, operational stability, and adaptability to external disturbances in complex environments.

[0165] In summary, this invention provides a high-precision dynamic trajectory control method and device for SCARA robots. Through the deep integration of kinematic analysis, dynamic modeling, parameter identification, composite controller design, and multi-source sensor feedback, a closed-loop control system integrating accuracy and adaptability is constructed. The system first accurately solves for the initial and target poses based on the homogeneous transformation matrix, and improves pose positioning accuracy by combining an error compensation mechanism. Then, a Lagrangian dynamic model incorporating friction, inertia, and external disturbances is established, and an improved stochastic weighted particle swarm optimization (RWPSO) algorithm is introduced for parameter identification. With the synergy of diversity preservation and convergence speed monitoring mechanisms, premature convergence is effectively avoided, significantly improving identification efficiency and model accuracy. Based on this, an adaptive controller integrating the robustness of sliding mode control and the self-learning capability of the RBF neural network is designed, capable of real-time compensation for model uncertainties and external disturbances, achieving high-speed and high-precision trajectory tracking. During control execution, the system comprehensively perceives the operating status through multiple sensors, including vision, force, current, temperature, and torque sensors. Combined with preset trajectory generation, real-time closed-loop control, precise end-effector alignment, and dynamic parameter adjustment mechanisms, a complete control chain of perception, decision-making, compensation, and execution is formed. Whether the trajectory deviation is caused by load changes, thermal expansion, or external disturbances, the system can identify and correct it in real time through adaptive compensation strategies, ensuring that the end-effector moves stably and accurately along the preset trajectory and ultimately achieves precise positioning. This invention not only significantly improves the trajectory accuracy, response speed, and operational stability of SCARA robots under complex working conditions but also enhances their adaptability and reliability in demanding scenarios such as industrial assembly and precision manufacturing, demonstrating promising engineering application prospects and widespread application value.

[0166] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the invention. The above descriptions are merely preferred embodiments of the invention and are not intended to limit the invention. It should be noted that any modifications, equivalent substitutions, and improvements made within the spirit and principles of the invention should be included within the scope of protection of the invention.

Claims

1. A dynamic trajectory control method for a SCARA robot, characterized in that, The method includes the following steps: Based on the kinematic model of the SCARA robot, the coordinate transformation relationship of each joint and link in space is analyzed to determine the robot's initial pose and target pose; the coordinate transformation relationship is represented by a homogeneous transformation matrix. A dynamic model of the SCARA robot is established based on the Lagrange equation to determine the dynamic parameters that need to be identified. The dynamic parameters are identified using a stochastic weighted particle swarm optimization algorithm, and dynamic parameter identification results are generated. Based on the identified dynamic parameters, a sliding mode robust RBF neural network adaptive controller is constructed. The controller performs real-time control of the SCARA robot's trajectory, enabling the robot's end effector to move along a preset dynamic trajectory to the target pose. It detects whether the end effector has reached the target pose; if not, it adjusts the controller parameters until the end effector reaches the target pose. The detection process uses sensors to acquire the end effector's real-time pose information and compares it with the target pose. When the error is less than a preset threshold, it is determined that the target pose has been reached.

2. The SCARA robot dynamic trajectory control method as described in claim 1, characterized in that, The step of identifying the dynamic parameters using the stochastic weighted particle swarm optimization algorithm includes: The particle swarm is initialized, with each particle representing a possible set of dynamic parameters; the initial positions of the particle swarm are randomly initialized based on prior knowledge of the dynamic parameters to ensure the global search capability of the algorithm. The fitness value of each particle is calculated based on the error between the output of the dynamic model and the actual measured value; the error is calculated using the root mean square error (RMSE), and its expression is as follows: in, For the first One actual measured value, Output values ​​for the model. The total number of samples; Update the particle's position and velocity, where the velocity update formula is: The position update formula is: in, and The first The particle in the first Velocity and position at the next iteration For inertial weights, and As a learning factor, A random number within the interval [0,1]. For particles The optimal position of an individual The global optimal position; the inertial weight During the iteration process, adjustments are made according to a linear decreasing strategy to balance global search capability and local search capability; After satisfying the preset convergence conditions, the final dynamic parameter identification results are output; the convergence conditions include the rate of change of fitness value being less than a preset threshold or the number of iterations reaching a preset upper limit.

3. The SCARA robot dynamic trajectory control method as described in claim 1 or 2, characterized in that, The steps involved in designing a sliding mode robust RBF neural network adaptive controller include: An RBF neural network is constructed, with the robot's joint angles, velocities, and accelerations as inputs and control torque as outputs. The center and width of the RBF neural network are determined using a K-means clustering algorithm to improve the network's generalization ability. A sliding mode control law is constructed and combined with the output of an RBF neural network to generate the final control signal. The sliding mode control law is designed using nonlinear functions to reduce jitter during the sliding mode control process. The control signals are used to control the joints of the SCARA robot in real time, so that the robot's end effector moves along a preset dynamic trajectory.

4. The SCARA robot dynamic trajectory control method as described in claim 1, characterized in that, The step of detecting whether the end effector has reached the target pose includes: The real-time pose information of the end effector is acquired; the real-time pose information is acquired through a high-precision six-dimensional force sensor and a vision sensor installed on the end effector. The error between the real-time pose and the target pose is calculated. If the error is less than a preset threshold, it is determined that the end effector has reached the target pose. The error calculation includes position error and attitude error, and different thresholds are set for each.

5. The SCARA robot dynamic trajectory control method as described in claim 1 or 4, characterized in that, The method further includes: After the end effector reaches the target pose, a second control command is generated based on the distance between the end effector and the target pose. The end effector is precisely aligned with the target pose by the motors corresponding to the robot's moving joints based on the second control command; the second control command is generated by a proportional-integral-derivative controller.

6. The SCARA robot dynamic trajectory control method as described in claim 5, characterized in that, The steps for generating the second control command include: Based on the distance between the end effector and the target pose and the preset ratio coefficient Calculate the control torque: The proportionality coefficient Dynamically adjust based on the load and movement speed of the end effector; The control torque This is sent as a second control command to the motor corresponding to the moving joint.

7. The SCARA robot dynamic trajectory control method as described in claim 1, characterized in that, Also includes: During the movement of the end effector along a preset dynamic trajectory, the joint torque of the robot is monitored in real time, and the parameters of the controller are adjusted according to the joint torque to compensate for external interference. The step of real-time monitoring of the robot's joint torque includes: The robot's joint torque data are acquired through sensors; Based on the joint torque data and the preset compensation strategy, the parameters of the controller are adjusted to ensure that the end effector moves stably along the preset dynamic trajectory.

8. A dynamic trajectory control device for a SCARA robot, characterized in that, The device includes: The kinematics analysis module is used to analyze the coordinate transformation relationship of each joint and link in space based on the kinematic model of the SCARA robot, and to determine the robot's initial pose and target pose. The dynamics modeling module is used to build a dynamics model of the SCARA robot based on the Lagrange equation and determine the dynamic parameters that need to be identified. The parameter identification module is used to identify the dynamic parameters using a random weighted particle swarm optimization algorithm and generate dynamic parameter identification results. The parameter identification module includes a particle swarm initialization unit, a fitness calculation unit, a particle update unit, and an identification result output unit. The controller fabrication module is used to design a sliding mode robust term RBF neural network adaptive controller based on the dynamic parameter identification results; the controller fabrication module includes a neural network construction unit, a sliding mode control law design unit, and a control signal generation unit. The trajectory control module is used to control the trajectory of the SCARA robot in real time through the controller, so that the robot's end effector moves to the target pose along a preset dynamic trajectory; the trajectory control module includes a preset trajectory generation unit and a real-time control unit. The detection module is used to detect whether the end effector has reached the target pose. If it has not reached the target pose, the parameters of the controller are adjusted until the end effector reaches the target pose. The detection module includes a pose acquisition unit and an error calculation unit to accurately determine whether the end effector has reached the target pose.

9. The SCARA robot dynamic trajectory control device as described in claim 8, characterized in that, The parameter identification module further includes: A diversity preservation unit is used to maintain the diversity of particles during particle swarm optimization to prevent the particle swarm from getting trapped in local optima. The diversity preservation unit achieves this by randomly perturbing the position and velocity of some particles. When the diversity of the particle swarm is lower than a preset threshold, the diversity preservation mechanism is automatically triggered. The convergence speed monitoring unit is used to monitor the convergence speed of the particle swarm optimization algorithm and dynamically adjust the inertia weight and learning factor according to the convergence speed. The convergence speed monitoring unit determines the convergence speed by calculating the rate of change of fitness value in continuous iterations. When the convergence speed is lower than a preset threshold, the inertia weight and learning factor are automatically adjusted to accelerate the convergence speed.

10. The SCARA robot dynamic trajectory control device as described in claim 8 or 9, characterized in that, The device further includes: The pose detection module is used to acquire the real-time pose information of the end effector and calculate the error between the real-time pose and the target pose. If the error is less than a preset threshold, it is determined that the end effector has reached the target pose. The precision alignment module is used to generate a second control command based on the distance between the end effector and the target pose after the end effector reaches the target pose, and control the end effector to precisely align with the target pose based on the second control command through the motors corresponding to the robot's moving joints; The interference compensation module is used to monitor the joint torque, joint temperature and joint current of the robot in real time during the movement of the end effector along a preset dynamic trajectory, and adjust the parameters of the controller according to the joint torque, joint temperature and joint current to compensate for the impact of external interference and environmental changes on trajectory accuracy.

Images