A trajectory tracking method for dual-arm humanoid robot based on improved terminal sliding mode algorithm

Abstract

This invention relates to a trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm. This method addresses the problem in existing technologies where neglecting joint flexibility leads to controllers being prone to high-frequency vibrations and reduced robustness. It establishes a state-space model incorporating flexible dynamics and extracts the instantaneous vibration frequencies and damping characteristic parameters of the system online. Using these characteristic parameters, an adaptive non-singular terminal sliding mode surface with frequency avoidance and damping injection capabilities is dynamically constructed. A linearly extended state observer is combined to perform real-time estimation and feedforward compensation of the system's lumped disturbances, designing a continuous and smooth composite control law. The above steps are integrated through a closed-loop execution process, achieving high-precision tracking of the desired trajectory while effectively suppressing residual vibrations of the flexible joints, significantly improving the system's stability and robustness under complex operating conditions.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, specifically to a trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm. Background Technology

[0002] As a cutting-edge field of robotics, dual-arm humanoid robots, due to their high structural similarity to human arms, exhibit irreplaceable advantages in scenarios such as precision operations within spacecraft and dexterous work in complex environments. Achieving high-precision, high-dynamic tracking of a preset trajectory by its end effector is fundamental to accomplishing these tasks. In this control problem, the sliding mode control algorithm for the end effector has been widely applied in robot trajectory tracking in recent years due to its ability to converge within a finite time and its inherent robustness to system parameter perturbations and external disturbances. It is considered one of the key technologies for improving the dynamic performance and anti-interference capabilities of robots.

[0003] However, most existing robot trajectory tracking methods based on end-effector sliding mode are based on the assumption that joints are ideally rigidly connected. In practical robot systems, harmonic reducers and other transmission components are widely used to achieve high reduction ratios and reduce backlash. This inevitably introduces significant joint flexibility, resulting in complex flexible coupling dynamics between the drive motor and the linkage. Traditional end-effector sliding mode controllers do not fully integrate this flexible dynamic characteristic in their design. Their high-gain switching control law easily excites the high-frequency, low-damped vibration modes inherent in joint flexibility, leading to two prominent problems: First, it induces continuous mechanical resonance during actual operation, which not only generates noise and affects positioning accuracy but also damages the service life of transmission components; second, to suppress observed vibrations, the control gain is often forced to be reduced in engineering, which directly sacrifices the original fast convergence characteristics and robustness to uncertainties of sliding mode control. In microgravity environments, where external disturbances are complex and varied, or where dynamic coupling effects are severe during high-speed motion, the mismatch between the aforementioned "rigid controller" and "flexible object" becomes particularly serious, leading to a significant decrease in system robustness and making it difficult to simultaneously ensure the accuracy of trajectory tracking and the stability of the mechanical system. Summary of the Invention

[0004] Based on this, the purpose of this invention is to provide a trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm that can synergistically suppress flexible joint vibration and maintain strong robust trajectory tracking performance.

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

[0006] In a first aspect, the present invention provides a trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm, comprising the following steps:

[0007] S1: Model the dual-arm robot system with flexible joints. Based on the Lagrange equation and the principle of motor-link separation, establish the augmented dynamic equation containing the dynamic coupling relationship between the motor and the link, and generate a state-space model containing flexible dynamic characteristics and system lumped disturbance terms.

[0008] S2: Perform online vibration modal analysis on the state-space model. Linearize and decompose the state-space model at the current system state and the desired trajectory, extract the dominant vibration frequency and the equivalent damping ratio, and generate real-time vibration characteristic parameters.

[0009] S3: Adaptive construction of sliding surface for real-time vibration characteristic parameters, adjustment of convergence gain based on vibration frequency in real-time vibration characteristic parameters and injection of nonlinear damping based on damping ratio, generating non-singular terminal sliding surface and sliding variable that are adaptive to frequency and damping.

[0010] S4: Design a composite control law for the state-space model, non-singular terminal sliding surface and sliding variables. Combine a linear extended state observer to estimate and compensate for the lumped disturbance term of the system. Estimate the lumped disturbance value online and feed it forward to compensate to the control law. Combine the equivalent control term derived based on Lyapunov stability to generate a continuous and smooth joint motor control torque.

[0011] S5: Based on the execution feedback data of the joint motor control torque, perform closed-loop control processing, integrate the deviation between the current joint's actual position, speed and the desired trajectory, and generate a closed-loop trajectory control signal. The closed-loop trajectory control signal is used to instruct the joint motor to adjust the output torque.

[0012] In one embodiment, S1 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0013] S11: Perform rigid-flexible coupling dynamic modeling on the rigid link and flexible joint components of the dual-arm robot system containing flexible joints, obtain the physical structure parameters, mass inertia properties and joint stiffness and damping characteristics of the dual-arm robot system, and derive the motor rotor dynamic equation and the link dynamic equation considering joint elasticity based on the Lagrange equation, respectively, to generate a set of coupled dynamic equations including position, velocity and joint elastic force.

[0014] S12: Based on the coupled dynamic equations, the system state vector is augmented and the equations are first-order processed. By combining the motor position, the link position and their respective first derivatives into a new high-order state vector, an augmented state vector containing the position and velocity of the motor side and the link side is introduced. All the second-order differential equations in the coupled dynamic equations are rewritten into matrix differential equations with the augmented state vector and its first derivative as variables, generating the augmented state space equations of the dual-arm robot system.

[0015] S13: Perform a structured decomposition of the augmented state-space equations, explicitly decomposing the right-hand side of the augmented state-space equations into nonlinear function terms that are only related to the state, input matrix terms that are related to the control input, and lumped terms that include unmodeled dynamics and external disturbances. This generates a state-space model that includes flexible dynamic characteristics and lumped disturbance terms. The state-space model is used to indicate the exact mathematical model required for controller design and the sources of disturbances to be compensated.

[0016] In one embodiment, S2 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0017] S21: The state-space model is locally linearized at the current operating point. Taking the actual system state and the expected trajectory at the current moment as reference points, the Jacobian matrix of the nonlinear dynamic function of the system with respect to the augmented state vector is calculated to generate a linear time-varying system matrix that characterizes the instantaneous dynamics of the system.

[0018] S22: Perform eigenvalue analysis and modal decoupling on the linear time-varying system matrix. Solve all the eigenvalues ​​of the linear time-varying system matrix using numerical methods, and identify all conjugate complex root pairs from the complex eigenvalues. Each conjugate complex root pair corresponds to a vibration mode of the system, generating the eigenvalue set of the instantaneous vibration modes of the system.

[0019] S23: Extract characteristic parameters for each vibration mode in the eigenvalue set. Calculate the damping ratio and undamped natural frequency of the vibration mode based on the real and imaginary parts of the conjugate complex roots. Select the dominant modes that have a significant impact on the system dynamics. Categorize and organize the frequencies and damping ratios of the selected dominant modes to generate real-time vibration characteristic parameters composed of the dominant vibration frequency characteristic quantity and the equivalent damping ratio characteristic quantity.

[0020] In one embodiment, S3 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0021] S31: Based on the real-time vibration characteristic parameters, the dominant vibration frequency component that characterizes the intensity of vibration of each joint and the equivalent damping ratio component that characterizes its oscillation attenuation capability are respectively analyzed to generate frequency characteristic quantities and damping characteristic quantities for independent adjustment and control behavior.

[0022] S32: Based on frequency and damping characteristics, online calculation of sliding surface gain and damping term is performed. For the dominant vibration frequency component, the time-varying convergence gain corresponding to each joint is calculated by mapping through the negative correlation function, and a time-varying convergence gain diagonal matrix is ​​constructed. For the equivalent damping ratio component, the additional damping value required to compensate for the insufficient inherent damping of the system is calculated by the compensation function, and a time-varying damping injection diagonal matrix is ​​constructed to generate an adaptive gain matrix and damping matrix.

[0023] S33: Combine the adaptive gain matrix, damping matrix, and the system's position tracking error and error derivative obtained from sensor feedback and trajectory comparison processing according to the preset terminal attractor structure to form a complete sliding surface function expression. Substitute the current error state into the sliding surface function expression for calculation to generate a non-singular terminal sliding surface function with adaptive frequency and damping and its corresponding sliding variable value at the current moment.

[0024] In one embodiment, S4 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0025] S41: Based on the state-space model, the differential relationship of the non-singular terminal sliding surface, and the sliding variables, the stability derivation of the equivalent control law is performed. Based on the Lyapunov stability theory, an energy function is constructed. The derivative of the sliding variables is set to zero and the lumped disturbance is ignored. The control input required to maintain the ideal sliding mode is solved from the state-space model, and the equivalent control term that ensures the system trajectory approaches the sliding surface is generated.

[0026] S42: Design and perform online estimation of the extended state observer for the lumped disturbance term defined in the state-space model. Construct a linear observer that takes the system's measurables as input and expands the lumped disturbance term into a new state. Configure the observer's poles to quickly track the true value of the disturbance and generate a real-time disturbance estimate of the lumped disturbance term.

[0027] S43: The equivalent control term and the real-time disturbance estimate are fed forward to compensate and synthesize and the signal is smoothed. The real-time disturbance estimate is dynamically scaled by the inverse of the adaptive gain matrix and then fed forward to the equivalent control term. The synthesized signal is replaced by a continuous saturation function to eliminate high-frequency chattering and generate a continuous and smooth joint motor control torque.

[0028] In one embodiment, S5 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0029] S51: The process of executing the joint motor control torque is processed by collecting and fusing feedback data from multiple sources of sensors. The actual position, actual speed and actual output torque of the motor measured by the joint encoder and current sensor are obtained in real time, and an execution feedback dataset reflecting the instantaneous motion state of the system is generated.

[0030] S52: Obtain the expected position and expected velocity of each joint at the current moment from the expected trajectory planner, and perform corresponding component subtraction and weighted fusion with the actual position and actual velocity in the execution feedback dataset to generate a fused tracking deviation signal for closed-loop control decision-making;

[0031] S53: Perform closed-loop control signal synthesis and output command generation processing on the fused tracking deviation signal. The fused tracking deviation signal is used as the core feedback quantity. According to the preset control logic, it is mapped to the adjustment command of the joint motor output torque, and a closed-loop trajectory control signal is generated to directly instruct the joint servo driver to adjust the output torque.

[0032] Secondly, this application provides a computer device, including a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements any of the above-mentioned methods for tracking the trajectory of a dual-arm humanoid robot based on an improved terminal sliding mode algorithm.

[0033] Thirdly, this application provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements any of the above-mentioned methods for trajectory tracking of a dual-arm humanoid robot based on an improved terminal sliding mode algorithm.

[0034] In summary, the trajectory tracking method for dual-arm humanoid robots based on an improved terminal sliding mode algorithm provided by this invention can fundamentally characterize the real physical behavior of the system by establishing a precise dynamic model that integrates the flexible characteristics of the joints. Based on this model, the vibration modes are analyzed online and frequency and damping features are extracted, enabling real-time perception of the system's "vulnerability points." By dynamically constructing an adaptive sliding surface using these feature parameters, the convergence gain can be automatically adjusted according to vibration risk, and nonlinear damping can be actively injected, thereby avoiding resonance and enhancing dissipation at the control law level. Combined with a linear extended state observer for feedforward compensation of lumped disturbances, the high gain required to overcome uncertainties can be significantly reduced, effectively smoothing the control signal to suppress the chattering phenomenon inherent in traditional sliding mode control from the source. By integrating the above steps to form a closed-loop control, the mechanical vibration caused by flexible joints can be actively and coordinately suppressed while ensuring trajectory tracking accuracy, thereby significantly improving the overall stability and robustness of the dual-arm robot in complex dynamic scenarios such as microgravity and high-speed motion.

[0035] To better understand and implement this invention, the following detailed description is provided in conjunction with the accompanying drawings. Attached Figure Description

[0036] Figure 1 A flowchart illustrating a trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm, provided for an embodiment of this application;

[0037] Figure 2 This is a schematic diagram of the process for generating real-time vibration characteristic parameters provided in an embodiment of this application. Detailed Implementation

[0038] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Preferred embodiments of the invention are shown in the drawings. However, the invention can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a thorough and complete understanding of the disclosure of the invention.

[0039] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items.

[0040] In one embodiment, such as Figure 1As shown, a trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm is provided. This embodiment illustrates the application of this method to a terminal. It is understood that this method can also be applied to a server, and to a system including both a terminal and a server, and implemented through interaction between the terminal and the server. In this embodiment, the method includes the following steps:

[0041] S1: Model the dual-arm robot system with flexible joints. Based on the Lagrange equation and the principle of motor-link separation, establish the augmented dynamic equations that include the dynamic coupling relationship between the motor and the link, and generate a state-space model that includes flexible dynamic characteristics and system lumped disturbance terms.

[0042] Specifically, this application takes a dual-arm humanoid robot system as the core control object and achieves high-precision trajectory tracking in flexible joint scenarios through a continuous process of dynamic modeling, vibration modal analysis, adaptive sliding surface construction, composite control law design, and closed-loop feedback control. Specifically, the system models the dual-arm robot system containing flexible joints, using the Lagrange energy conservation equation as the core theoretical basis and combining it with the motor-link separation principle to construct the dynamic equations. The application of the motor-link separation principle is based on the flexible characteristics introduced by the transmission components at the joints of the dual-arm robot, dividing the robot's motion execution structure into two coupled dynamic units: the motor rotor and the mechanical link. The system analyzes and calculates the kinetic energy, potential energy, and work done by non-conservative forces in each unit, clarifying the mechanical transmission relationship between the motor rotor and the mechanical link. The system incorporates the dynamic influence of joint flexible deformation during the analysis process, establishing multi-dimensional state variables including motor rotation angle, motor angular velocity, link rotation angle, link angular velocity, and joint flexible deformation. The system integrates uncertainties such as system parameter perturbations, external environmental disturbances, and unmodeled dynamics into its dynamic equations, defining them as lumped disturbance terms. Through the selection of state variables and equation transformation, the dynamic equations are converted into a state-space model that includes flexible dynamic characteristics and lumped disturbance terms. The state-space model can fully reflect the kinematic and dynamic characteristics of the dual-arm robot system, accurately represent the coupling relationship between the motor and the linkage, and the effects of various uncertainties on the system. The establishment of this model provides a foundation for subsequent vibration modal analysis and control law design. The system does not rely on specific structural and operational parameters during the modeling process, but only on the basic principles of dynamics and the structural characteristics of the system to complete the modeling work, ensuring that the model has universality and adaptability.

[0043] S2: Perform online vibration modal analysis on the state-space model. Linearize and decompose the state-space model at the current system state and the desired trajectory, extract the dominant vibration frequency and equivalent damping ratio, and generate real-time vibration characteristic parameters.

[0044] Specifically, the system performs online vibration modal analysis based on the established state-space model. The dual-arm robot system is a strongly nonlinear coupled system, and its vibration characteristics are directly related to the system's operating state. The system selects the equilibrium point corresponding to the current system state and the desired trajectory as the linearization reference point. The system uses Taylor expansion to perform local linearization on the state-space model, transforming the nonlinear system into a linear system that can be analyzed by eigenvalues. The system performs eigenvalue decomposition on the system matrix of the linearized state-space model, obtaining the system's eigenvalue set. The real part of the complex eigenvalues ​​corresponds to the equivalent damping ratio of the system vibration, and the imaginary part of the complex eigenvalues ​​corresponds to the angular frequency of the system vibration. The system extracts the eigenvalues ​​that play a dominant role in the system's dynamic characteristics by filtering the amplitude of the eigenvalues. The system calculates the corresponding dominant vibration frequency and equivalent damping ratio based on the extracted dominant eigenvalues, thereby generating real-time vibration characteristic parameters. These real-time vibration characteristic parameters can reflect the vibration characteristics of the flexible joints under the current system operating state.

[0045] S3: Adaptive construction of sliding surface for real-time vibration characteristic parameters, adjustment of convergence gain based on vibration frequency in real-time vibration characteristic parameters and injection of nonlinear damping based on damping ratio, generating non-singular terminal sliding surface and sliding variable that are adaptive to frequency and damping.

[0046] Specifically, to avoid the singularity problem of traditional terminal sliding surfaces, the system constructs a non-singular terminal sliding surface. The system constructs the sliding surface based on the link position tracking deviation and its derivative. Convergence gain and nonlinear damping coefficient are introduced into the sliding surface. The system avoids the singularity problem at the origin by setting the parameter relationship between the convergence gain and the nonlinear damping coefficient. The system establishes a correlation adjustment rule between the convergence gain and the dominant vibration frequency. When the dominant vibration frequency increases, the system proportionally reduces the value of the convergence gain to avoid high-gain control of the high-frequency vibration mode of the excitation system. When the effective damping ratio decreases, the system proportionally increases the convergence gain to ensure the rapid convergence characteristics of the sliding mode control. The system also establishes a correlation adjustment rule between the nonlinear damping coefficient and the equivalent damping ratio. When the equivalent damping ratio decreases, the system increases the value of the nonlinear damping coefficient to suppress system vibration by injecting stronger nonlinear damping. When the equivalent damping ratio increases, the system decreases the value of the nonlinear damping coefficient to reduce the impact of the damping element on the system's dynamic response. Based on the above adjustment rule, the system dynamically adjusts the convergence gain and the nonlinear damping coefficient according to the real-time vibration characteristic parameters to generate a non-singular terminal sliding mode surface with adaptive frequency and damping.

[0047] Furthermore, the system defines sliding variables based on the sliding surface. As the core control variables for subsequent composite control law design, the sliding variables can reflect the comprehensive information of the system's trajectory tracking deviation and vibration state in real time. The entire adaptive construction process of the sliding surface is automatically completed by the system based on real-time vibration characteristic parameters. The system can adjust the sliding surface parameters in real time according to the changes in vibration state, ensuring that the sliding surface can both suppress the vibration of the flexible joint and ensure the speed of trajectory tracking, thereby achieving synergistic optimization of vibration suppression and trajectory tracking performance and avoiding the performance imbalance problem caused by the fixed parameters of the traditional sliding surface.

[0048] S4: Design a composite control law for the state-space model, non-singular terminal sliding surface and sliding variables. Combine a linear extended state observer to estimate and compensate for the lumped disturbance term of the system. Estimate the lumped disturbance value online and feed it forward to compensate to the control law. Combine the equivalent control term derived based on Lyapunov stability to generate a continuous and smooth joint motor control torque.

[0049] Specifically, to achieve accurate estimation and compensation of the lumped disturbance term, the system introduces a linearly extended state observer to observe the lumped disturbance term in the state-space model. The system designs the observer's state observation vector, state differential observation vector, and lumped disturbance observation vector. The system uses the pole placement method to design the observer's gain matrix to ensure fast convergence and stability. The system acquires the lumped disturbance observation vector in real time through the observer. The system introduces the lumped disturbance observation vector as a feedforward compensation term into the control law. Simultaneously, the system derives an equivalent control term based on Lyapunov stability theory. The system constructs a Lyapunov function and differentiates it. The system ensures stability by ensuring that the derivative of the Lyapunov function is less than or equal to zero. The system also incorporates the sliding mode variables... The equivalent control term is derived by substituting the differential expression into the state-space model. The system combines the equivalent control term with the lumped disturbance feedforward compensation term, and introduces a saturation function to weaken the chattering phenomenon of traditional sliding mode control, generating a continuous and smooth joint motor control torque. The introduction of the saturation function can avoid abrupt changes in control torque, ensuring the continuity and smoothness of control torque, and thus avoiding system vibration caused by abrupt changes in control torque. The entire composite control law design process strictly follows stability theory to ensure that the generated control torque can effectively compensate for lumped disturbances and ensure system stability, while avoiding the impact of control chattering on the system. This solves the vibration excitation problem caused by high gain switching in traditional terminal sliding mode control, achieving a balance between control performance and system stability.

[0050] S5: Based on the execution feedback data of the joint motor control torque, perform closed-loop control processing, integrate the deviation between the current joint's actual position, speed and the desired trajectory, and generate a closed-loop trajectory control signal. The closed-loop trajectory control signal is used to instruct the joint motor to adjust the output torque.

[0051] Specifically, the joint motor receives the control torque signal and outputs the corresponding torque to drive the linkage motion. The system collects the actual position and velocity of the joint in real time through position and velocity sensors. The system calculates the position and velocity deviations between the actual joint position and velocity and the desired trajectory, obtaining position deviation vectors and velocity deviation vectors. The system feeds back the collected joint position and velocity data to the online vibration modal analysis stage to update the real-time vibration characteristic parameters. Simultaneously, the system feeds back the position and velocity deviation vectors to the sliding surface adaptive construction stage to update the sliding variables. The system inputs the feedback data to the linear expansion state observer to update the lumped disturbance observation vector. Based on the real-time updates of the above feedback data, the system recalculates the joint motor control torque and generates a closed-loop control mechanism. The closed-loop trajectory control signal instructs the joint motors to adjust their output torque in real time. Through the aforementioned closed-loop feedback mechanism, the system achieves real-time correction of trajectory tracking deviations while continuously suppressing the vibration of the flexible joints. Through continuous feedback and adjustment, the system ensures that the end effector of the dual-arm humanoid robot always tracks the preset trajectory. The entire closed-loop control process forms a complete control closed loop. The system can adjust the control strategy in real time according to external disturbances and changes in system parameters, ensuring high accuracy in trajectory tracking and stable system operation. Ultimately, it enables the end effector of the dual-arm humanoid robot to achieve high-precision and high-dynamic tracking of the preset trajectory, meeting the requirements for robot trajectory tracking performance in scenarios such as precision operations in space capsules and dexterous operations in complex environments.

[0052] In summary, the trajectory tracking method for dual-arm humanoid robots based on an improved terminal sliding mode algorithm provided by this invention can fundamentally characterize the real physical behavior of the system by establishing a precise dynamic model that integrates the flexible characteristics of the joints. Based on this model, the vibration modes are analyzed online and frequency and damping features are extracted, enabling real-time perception of the system's "vulnerability points." By dynamically constructing an adaptive sliding surface using these feature parameters, the convergence gain can be automatically adjusted according to vibration risk, and nonlinear damping can be actively injected, thereby avoiding resonance and enhancing dissipation at the control law level. Combined with a linear extended state observer for feedforward compensation of lumped disturbances, the high gain required to overcome uncertainties can be significantly reduced, effectively smoothing the control signal to suppress the chattering phenomenon inherent in traditional sliding mode control from the source. By integrating the above steps to form a closed-loop control, the mechanical vibration caused by flexible joints can be actively and coordinately suppressed while ensuring trajectory tracking accuracy, thereby significantly improving the overall stability and robustness of the dual-arm robot in complex dynamic scenarios such as microgravity and high-speed motion.

[0053] In one embodiment, S1 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0054] S11: Perform rigid-flexible coupling dynamic modeling on the rigid links and flexible joint components of the dual-arm robot system containing flexible joints, obtain the physical structural parameters, mass inertia properties and joint stiffness and damping characteristics of the dual-arm robot system, and derive the motor rotor dynamic equation and the link dynamic equation considering joint elasticity based on the Lagrange equation, respectively, to generate a set of coupled dynamic equations including position, velocity and joint elastic force.

[0055] Specifically, the system acquires the physical structural parameters, mass inertia attributes, and joint stiffness and damping characteristics of the dual-arm robot system. The physical structural parameters include the geometric dimensions of the rigid links and the structural form of the joints. The mass inertia attributes include the distributed mass and concentrated mass of the rigid links, as well as the rotational inertia of the motor rotor. The joint stiffness and damping characteristics include the stiffness and damping characteristics of the equivalent torsional spring of the flexible joint. Based on the Lagrange equations as the core theoretical foundation, the system derives the dynamic equations for both the motor rotor and the rigid links considering joint elasticity. In deriving the motor rotor's dynamic equations, the system considers the interaction of the electromagnetic torque, joint elastic reaction force, and damping torque acting on the motor rotor. In the process of formulating the dynamic equations of the rigid link with joint elasticity, the system considers the relationship between the joint elastic force, gravity, Coriolis force and centrifugal force acting on the rigid link. Based on the characteristic that the joint elastic force is the interaction force between the motor rotor and the rigid link, the system couples the derived motor rotor dynamic equations with the rigid link dynamic equations, eliminates the intermediate variable of joint elastic force in the equations, and generates a set of coupled dynamic equations that include the motor rotor position, motor rotor speed, rigid link position, rigid link speed and joint elastic force. This set of coupled dynamic equations fully reflects the rigid-flexible coupling relationship between the rigid link and the flexible joint component, and accurately characterizes the dynamic characteristics of the dual-arm robot system containing flexible joints.

[0056] S12: Based on the coupled dynamics equations, the system state vector is augmented and the equations are first-order processed. By combining the motor position, link position and their respective first derivatives into a new high-order state vector, an augmented state vector containing the position and velocity of the motor side and the link side is introduced. All the second-order differential equations in the coupled dynamics equations are rewritten into matrix differential equations with the augmented state vector and its first derivative as variables, generating the augmented state space equations of the dual-arm robot system.

[0057] Specifically, the system analyzes the structural characteristics of the coupled dynamic equations, identifies the second-order differential variables containing motor position and link position, and combines the motor position, its first derivative, link position, and link position's first derivative into a new higher-order state vector, completing the augmented definition of the system state vector. This augmented state vector can fully characterize the system's motion state. The system performs first-order transformation on all second-order differential equations in the coupled dynamic equations, decomposing each second-order differential equation into two first-order differential equations with the elements of the augmented state vector as variables. Specifically, the motor position... The second derivative and the second derivative of the link position are respectively expressed as the functional relationship between the augmented state vector elements and the control input. Based on the result of the first-order processing, the system integrates all equations in the coupled dynamics equation set into a unified matrix form, and rewrites them into a matrix differential equation form with the augmented state vector and its first derivative as variables, generating the augmented state space equation of the dual-arm robot system. This augmented state space equation realizes the equivalent transformation of the original coupled dynamics equation set, transforming the complex multivariable coupled second-order differential equation set into a first-order matrix differential equation that is convenient for controller design, while retaining all the dynamic information of the original equation set.

[0058] S13: Perform a structured decomposition of the augmented state-space equations, explicitly decomposing the right-hand side of the augmented state-space equations into nonlinear function terms that are only related to the state, input matrix terms that are related to the control input, and lumped terms that include unmodeled dynamics and external disturbances. This generates a state-space model that includes flexible dynamic characteristics and lumped disturbance terms. The state-space model is used to indicate the exact mathematical model required for controller design and the sources of disturbances to be compensated.

[0059] Specifically, the system performs a structured decomposition of the augmented state-space equations. The system analyzes the components of the right-hand side of the equations, explicitly decomposing it into three independent parts. The first part consists of nonlinear function terms related only to the augmented state vector. This part is determined by the system's inertial characteristics, Coriolis force characteristics, gravitational characteristics, and joint stiffness and damping characteristics, reflecting the inherent dynamic coupling relationships within the system. The second part consists of input matrix terms related to the control input. This part reflects the driving effect of the control input on the changes in the augmented state vector, establishing a linear mapping relationship between the control input and system state changes. The third part contains lumped terms including unmodeled dynamics and external disturbances. The unmodeled dynamics encompass joint friction nonlinearity and higher-order characteristics of component flexible deformation that were not considered during system modeling. External disturbances include contact force disturbances and environmental vibration disturbances in the working environment. Through the above-mentioned structured decomposition operation, the system generates a state-space model that includes flexible dynamic characteristics and lumped disturbance terms. This state-space model clearly distinguishes the inherent dynamic characteristics of the system, the control input, and the sources of disturbances. It is used to indicate the exact mathematical model required for controller design and the sources of disturbances to be compensated. This provides a clear target and basis for the design of disturbance observation and compensation links in the subsequent controller, ensuring that the controller can suppress disturbances in a targeted manner and achieve high-precision control by utilizing the inherent characteristics of the system.

[0060] In one embodiment, such as Figure 2 As shown, S2 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0061] S21: Approximate the state-space model with local linearization at the current operating point. Using the actual system state and the desired trajectory at the current moment as reference points, calculate the Jacobian matrix of the nonlinear dynamic function of the system with respect to the augmented state vector, and generate a linear time-varying system matrix that characterizes the instantaneous dynamics of the system.

[0062] Specifically, the system performs local linearization approximation on the state-space model at the current operating point, using the actual system state and desired trajectory at the current moment as reference points. The selection of these reference points must align with the system's real-time operating state and the target's motion trend. This selection method ensures that the linearized model accurately matches the system's current dynamic characteristics. The system dynamics function in the state-space model contains nonlinear terms. Directly performing vibration modal analysis based on the nonlinear model would make it difficult to meet the time requirements of real-time control. Therefore, the system needs to transform the nonlinear model into a linear model through local linearization. For the nonlinear dynamics function f(x) in the state-space model, the system calculates its Jacobian matrix with respect to the augmented state vector x according to the rules for solving partial derivatives of multivariate functions. The expression for the Jacobian matrix is:

[0063]

[0064] Where f(x) represents the nonlinear dynamic function in the state-space model, which comprehensively reflects the inherent dynamic characteristics of the system, such as inertia, Coriolis force, gravity, and joint stiffness and damping; x represents the augmented state vector, which includes motor position, motor angular velocity, link position, and link angular velocity; n represents the dimension of the augmented state vector. This represents the i-th element of the nonlinear dynamic function f(x), corresponding to the dynamic relationship of the i-th state variable; Let represent the j-th element of the augmented state vector x. The system will use the augmented state vector corresponding to the reference point at time t. Substitute into the Jacobian matrix By combining the nonlinear dynamic function value at the reference point, local linearization is performed to generate the linear time-varying system matrix A(t). The expression for the linear time-varying system matrix is:

[0065]

[0066] in, Let A(t) be the augmented state vector corresponding to the reference point at time t. The elements of the linear time-varying system matrix A(t) are dynamically updated with time t, always keeping in sync with the actual system state and the desired trajectory.

[0067] S22: Perform eigenvalue analysis and modal decoupling on the linear time-varying system matrix. Solve all the eigenvalues ​​of the linear time-varying system matrix using numerical methods, and identify all conjugate complex root pairs from the complex eigenvalues. Each conjugate complex root pair corresponds to a vibration mode of the system, generating the eigenvalue set of the instantaneous vibration modes of the system.

[0068] Specifically, the elements of the matrix in a linear time-varying system change dynamically with time. Analytical methods cannot directly solve for the eigenvalues ​​of such matrices. The system employs a numerical iterative method to solve for all eigenvalues ​​of the linear time-varying system matrix. This numerical iterative method can adapt to the time-varying characteristics of the matrix elements, ensuring the real-time performance and effectiveness of the eigenvalue solution process. Preferably, the expression for the characteristic equation is:

[0069]

[0070] in, This represents the determinant operation of a matrix, where I represents the identity matrix, and its dimension is the same as that of the linear time-varying system matrix. Consistent Represents the matrix of a linear time-varying system eigenvalues, The system matrix is ​​linear and time-varying. After solving the system and obtaining all eigenvalues, the eigenvalues ​​are classified and identified to distinguish between real and complex eigenvalues. Furthermore, conjugate complex root pairs are selected from the complex eigenvalues.

[0071] Based on vibration dynamics theory, real eigenvalues ​​correspond to the non-vibrational dynamic characteristics of a system, and conjugate complex root pairs correspond to the vibration modes of the system. Each pair of conjugate complex root pairs uniquely corresponds to an independent vibration mode. The system, through modal decoupling, decomposes the multimodal coupled vibration characteristics represented by the linear time-varying system matrix into multiple independent single vibration modes. Decoupling eliminates coupling interference between different vibration modes, allowing the characteristics of each vibration mode to be analyzed independently. The system then organizes and summarizes all identified conjugate complex root pairs to generate an eigenvalue set for the instantaneous vibration modes of the system. This eigenvalue set completely contains the core characteristic information of all vibration modes of the current system.

[0072] S23: Extract characteristic parameters for each vibration mode in the eigenvalue set. Calculate the damping ratio and undamped natural frequency of the vibration mode based on the real and imaginary parts of the conjugate complex roots. Select the dominant modes that have a significant impact on the system dynamics. Categorize and organize the frequencies and damping ratios of the selected dominant modes to generate real-time vibration characteristic parameters composed of the dominant vibration frequency characteristic quantity and the equivalent damping ratio characteristic quantity.

[0073] Specifically, the system performs feature parameter quantization extraction for each vibration mode in the eigenvalue set. For each pair of conjugate complex roots in the eigenvalue set, the system defines the expression for the conjugate complex root as follows:

[0074]

[0075] in, Let be the real part of the conjugate complex root. Let be the imaginary part of the conjugate complex root, and j be the imaginary unit. Based on the vibration dynamics characteristics, the system uses the real and imaginary parts of the conjugate complex root to calculate the damping ratio and undamped natural frequency of the corresponding vibration mode, respectively. The expression for calculating the damping ratio is:

[0076]

[0077] The expression for calculating the undamped natural frequency is:

[0078]

[0079] in, The damping ratio of the vibration mode is used to characterize the damping characteristics of the vibration mode. is the undamped natural frequency of the vibration mode, used to characterize the inherent frequency characteristics of the vibration mode; is the real part of the conjugate complex root; The imaginary part of the conjugate complex root is given. After calculating the damping ratio and undamped natural frequency of all vibration modes, the system analyzes the proportion of each vibration mode in the total vibration energy of the system, retaining vibration modes with a high proportion and significant impact on trajectory tracking accuracy and system stability as dominant modes. The system categorizes the undamped natural frequency corresponding to the selected dominant modes as frequency characteristic quantities and the corresponding damping ratio as equivalent damping ratio characteristic quantities. These two types of characteristic quantities are then summarized to generate real-time vibration characteristic parameters. These real-time vibration characteristic parameters provide real-time and effective state information for the subsequent adaptive construction of the sliding surface, ensuring that the control strategy can specifically address the current vibration state of the system.

[0080] In one embodiment, S3 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0081] S31: Based on the real-time vibration characteristic parameters, the dominant vibration frequency component that characterizes the intensity of vibration of each joint and the equivalent damping ratio component that characterizes its oscillation attenuation capability are analyzed respectively, and frequency characteristic quantities and damping characteristic quantities for independent adjustment and control behavior are generated.

[0082] Specifically, the system analyzes core components based on real-time vibration characteristic parameters to generate the characteristic quantities required for independent adjustment. First, the real-time vibration characteristic parameters are structurally analyzed, with the analysis process carried out on a joint-by-joint basis to achieve precise correlation between the characteristic parameters and each joint. The analysis objective is to separate two types of core components: one is the dominant vibration frequency component, which directly characterizes the intensity of vibration at the corresponding joint; the other is the equivalent damping ratio component, which directly characterizes the damping capability of the corresponding joint's oscillation motion. Preferably, the system assigns a joint number identifier 'i', whose value range is consistent with the total number of joints in the dual-arm robot. The dominant vibration frequency component corresponding to each joint 'i' is denoted as... The equivalent damping ratio component is denoted as Based on the above analysis results and settings, the system generates two sets of feature quantities: a set of frequency feature quantities and a set of damping feature quantities. The expression for the set of frequency feature quantities is:

[0083]

[0084] Preferably, the set of damping characteristic quantities is denoted as Its expression is:

[0085]

[0086] Where m represents the total number of joints in the dual-arm robot, and the superscript T indicates the transpose of the matrix. The set of frequency characteristics and the set of damping characteristics correspond to the core vibration characteristics of each joint, providing the state basis for the subsequent personalized and refined adjustment of the sliding surface parameters. This ensures that the control behavior of each joint can adapt to its own vibration state, avoiding control mismatch problems caused by uniform parameter adjustment.

[0087] S32: Based on frequency and damping characteristics, online calculation of sliding surface gain and damping term is performed. For the dominant vibration frequency component, the time-varying convergence gain corresponding to each joint is calculated through negative correlation function mapping, and a time-varying convergence gain diagonal matrix is ​​constructed. For the equivalent damping ratio component, the additional damping value required to compensate for the insufficient inherent damping of the system is calculated through compensation function, and a time-varying damping injection diagonal matrix is ​​constructed to generate adaptive gain matrix and damping matrix.

[0088] Specifically, the system performs online calculations of the sliding surface gain and damping terms based on frequency and damping characteristic quantities, and then constructs an adaptive matrix. The system processes a set of frequency characteristic quantities. For each joint i, the dominant vibration frequency component The time-varying convergence gain of the corresponding joint is calculated using a negative correlation function mapping relationship. The reason for choosing the negative correlation mapping is to avoid high-gain control of high-frequency vibration modes while ensuring fast convergence characteristics during low-frequency vibrations. The expression of the negative correlation function is as follows:

[0089]

[0090] in, Let be the time-varying convergence gain of the i-th joint. The convergence gain reference value, This is the frequency attenuation coefficient. Let be the dominant vibration frequency component of the i-th joint. The system arranges the time-varying convergence gains of all joints sequentially by joint number, constructing a time-varying convergence gain diagonal matrix. The construction method uses diagonal matrix construction operations, and the expression is as follows: Furthermore, the system processes the set of damping characteristic quantities Z, specifically the equivalent damping ratio component for each joint i. The additional damping value is calculated using a damping compensation function. This additional damping value is used to compensate for the insufficiency of the system's inherent damping. The expression for the damping compensation function is as follows:

[0091]

[0092] in, Let be the additional damping value for the i-th joint. This is the reference value for damping compensation. The preset target damping ratio, Let be the equivalent damping ratio component of the i-th joint. The system arranges the additional damping values ​​of all joints sequentially by joint number, and constructs a time-varying damping injection diagonal matrix using diagonal matrix construction operations. The expression is Through the above calculation and construction process, the system generates adaptive gain and damping matrices that adapt to the real-time vibration state of each joint.

[0093] S33: Combine the adaptive gain matrix, damping matrix, and the system's position tracking error and error derivative obtained from sensor feedback and trajectory comparison processing according to the preset terminal attractor structure to form a complete sliding surface function expression. Substitute the current error state into the sliding surface function expression for calculation to generate a non-singular terminal sliding surface function with adaptive frequency and damping and its corresponding sliding variable value at the current moment.

[0094] Specifically, the system collects the actual position and velocity signals of each joint using position and velocity sensors. The actual position signal is compared with the preset desired trajectory position signal to obtain the position tracking error *e*. The actual velocity signal is compared with the preset desired trajectory angular velocity signal to obtain the position tracking error derivative *ė*. The expression for the position tracking error *e* is:

[0095]

[0096] The expression for the derivative of the position tracking error, ė, is:

[0097]

[0098] in, Let q be the desired trajectory position vector, and q be the actual position vector. Let ω be the angular velocity vector of the desired trajectory. This represents the actual angular velocity vector. The system, according to the preset terminal attractor structure, will use the adaptive gain matrix... Damping matrix The position tracking error e and the position tracking error derivative ė are combined to form a complete non-singular terminal sliding surface function, expressed as:

[0099]

[0100] Where s is the sliding surface function, and p and q are coprime positive odd numbers that satisfy... This parameter relationship is used to ensure that the sliding surface function is not singular at the origin. The system substitutes the current position tracking error derivative ė and position tracking error e into the above sliding surface function expression, and combines it with the generated adaptive gain matrix. With damping matrix Numerical calculations are performed, and during the process, the generation of the non-singular terminal sliding surface function with adaptive frequency and damping is completed simultaneously, as well as the solution of the sliding variable value corresponding to the sliding surface function at the current moment. The sliding variable value comprehensively reflects the trajectory tracking error state of each joint and the vibration suppression requirements.

[0101] In one embodiment, S4 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0102] S41: Based on the state-space model, the differential relationship of the non-singular terminal sliding surface, and the sliding variables, the stability derivation of the equivalent control law is performed. Based on Lyapunov stability theory, an energy function is constructed. The derivatives of the sliding variables are set to zero and lumped disturbances are ignored. The control input required to maintain the ideal sliding mode is solved from the state-space model, and the equivalent control term that ensures the system trajectory approaches the sliding surface is generated.

[0103] Specifically, the system constructs an energy function based on Lyapunov stability theory, and the expression for the energy function is as follows:

[0104]

[0105] Where V is the Lyapunov energy function, s is the sliding mode variable value, and the superscript T indicates the matrix transpose operation. To ensure system stability, the derivative of the energy function must be less than or equal to zero. Taking the derivative of the energy function yields the product relationship between the derivative of the sliding mode variable and the sliding mode variable. The system sets the derivative of the sliding mode variable to zero to satisfy the constraint conditions of the ideal sliding mode. Simultaneously, it ignores the lumped disturbance term in the state-space model, substitutes the differential expression of the non-singular terminal sliding surface function into the state-space model, and solves the control input required to maintain the ideal sliding mode through matrix operations. This generates the equivalent control term that ensures the system trajectory approaches the sliding surface. The expression of the equivalent control term is:

[0106]

[0107] in, B is the equivalent control term, and B is the input matrix in the state-space model. Let f(x) be the ideal sliding mode variable derivative when the derivative of the sliding mode variable is equal to zero, and let f(x) be a nonlinear function term in the state-space model that is only related to the state. Let be the differential vector of the desired trajectory angular velocity. Let be the correlation vector of the deviation between the expected trajectory and the actual trajectory. Let be the desired trajectory angular acceleration vector, and let be the desired trajectory angular velocity vector. The derivative yields a value that reflects the dynamic trend of the desired trajectory. The adaptive convergent gain diagonal matrix, The adaptive damping is injected with a diagonal matrix, where p and q are coprime positive odd numbers.

[0108] S42: Design and perform online estimation of the extended state observer for the lumped disturbance term defined in the state-space model. Construct a linear observer that takes the system's measurables as input and expands the lumped disturbance term into a new state. By configuring the observer's poles, it can quickly track the true value of the disturbance and generate a real-time disturbance estimate of the lumped disturbance term.

[0109] Specifically, the system uses the measurable states in the state-space model as the observer input, expands the lumped disturbance term into new state variables, and constructs a linearly extended state observer. The state equation of the observer is:

[0110]

[0111] in, For the observations of the system's measurable state, For the observed values ​​of the system's measurable state derivative, For the observed values ​​of the lumped disturbance term, For system measurable state observations The first derivative, For the system's measurable state derivative observations The first derivative, Observations of the lumped disturbance term The first derivative, , , For observer gain, Let B be the measurable state of the system, and let B be the input matrix in the state-space model. To control the input, the system determines the observer gain using the pole placement method. , , This ensures that the observer's poles are located in the left half of the complex plane and have sufficiently negative real parts, guaranteeing that the observer can quickly track the true value of the disturbance. The system inputs the acquired system measurable state into the linearly extended state observer, and generates a real-time disturbance estimate of the lumped disturbance term through the observer's dynamic response process. .

[0112] S43: The equivalent control term and the real-time disturbance estimate are fed forward to compensate and synthesize and the signal is smoothed. The real-time disturbance estimate is dynamically scaled by the inverse of the adaptive gain matrix and then fed forward to the equivalent control term. The synthesized signal is replaced by a continuous saturation function to eliminate high-frequency chattering and generate a continuous and smooth joint motor control torque.

[0113] Specifically, the system will estimate the disturbance in real time. Dynamic scaling is performed using the inverse of the adaptive gain matrix K_α. The scaling aims to match the magnitude of the disturbance compensation term with that of the equivalent control term. The scaled disturbance compensation term is K_α^{-1}z_3. The scaled disturbance compensation term is then fed forward and superimposed onto the equivalent control term. The initial synthesized control signal is obtained. To eliminate the high-frequency chattering caused by the switching function in traditional sliding mode control, the system uses a continuous saturation function to replace the switching function to smooth the initial synthesized control signal. The expression of the saturation function is:

[0114]

[0115] Where sat(·) is a continuous saturation function, and s is the value of the sliding mode variable. Let be the boundary layer thickness of the saturation function, and sign(·) be the sign function. Through the above feedforward compensation synthesis and signal smoothing processing, a continuous and smooth joint motor control torque is generated. The expression for the control torque is:

[0116]

[0117] in, For joint motor control torque, Here is the sliding mode gain matrix. Adaptive gain matrix The inverse matrix, Let be the real-time estimate of the lumped disturbance term, sat(·) be the continuous saturation function, and s be the value of the sliding mode variable. The boundary layer thickness is a saturation function.

[0118] In one embodiment, S5 of the trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm provided by the present invention specifically includes the following steps:

[0119] S51: The process of executing the joint motor control torque is processed by collecting and fusing feedback data from multiple sources of sensors. The actual position, speed and output torque of the motor measured by the joint encoder and current sensor are obtained in real time, and an execution feedback dataset reflecting the instantaneous motion state of the system is generated.

[0120] Specifically, the system deploys two core sensor types: joint encoders and current sensors. The joint encoders are used to acquire the real-time position and speed signals of each joint motor, while the current sensors are used to acquire the armature current signals of the joint motors in real time and calculate the actual output torque using a motor electrical characteristic model. The system synchronously acquires raw data from both types of sensors at a preset sampling frequency. Outlier removal is performed on the raw data to eliminate abnormal data points caused by sensor noise or transmission interference. Then, a data fusion algorithm is used to fuse multi-source sensor data from the same joint, eliminating the uncertainty of single-sensor data. Finally, an execution feedback dataset containing the actual position, speed, and output torque of each joint motor is generated, which comprehensively reflects the instantaneous motion state of the system.

[0121] S52: Obtain the expected position and expected velocity of each joint at the current moment from the expected trajectory planner, and perform corresponding component subtraction and weighted fusion with the actual position and actual velocity in the execution feedback dataset to generate a fused tracking deviation signal for closed-loop control decision.

[0122] Specifically, the system extracts the desired position and desired velocity of each joint at the current moment from the desired trajectory planner, ensuring that the timestamp of the desired data is consistent with the sampling timestamp of the execution feedback data, thus avoiding inaccurate deviation calculations caused by time synchronization errors. The system subtracts the corresponding components of the actual position and desired position of each joint in the execution feedback dataset to obtain the position deviation component; it also subtracts the corresponding components of the actual velocity and desired velocity of each joint to obtain the velocity deviation component. The system uses preset weighting coefficients to perform weighted fusion of the position deviation and velocity deviation components. These weighting coefficients are determined based on the dynamic requirements of trajectory tracking and are used to balance the priorities of position tracking accuracy and velocity dynamic response. A fused tracking deviation signal for closed-loop control decision-making is generated through weighted summation, which comprehensively reflects the tracking error status at both the position and velocity levels.

[0123] S53: Perform closed-loop control signal synthesis and output command generation processing on the fused tracking deviation signal. The fused tracking deviation signal is used as the core feedback quantity. According to the preset control logic, it is mapped to the adjustment command of the joint motor output torque, and a closed-loop trajectory control signal is generated to directly instruct the joint servo driver to adjust the output torque.

[0124] Specifically, the system uses the generated fusion tracking deviation signal as the core feedback quantity, and calls the preset control logic to map the fusion tracking deviation signal. The preset control logic, based on the previously constructed state-space model and sliding mode control strategy, establishes a mapping relationship between the fusion tracking deviation and the adjustment amount of the joint motor's output torque, converting the fusion tracking deviation signal into a corresponding torque adjustment command. The system standardizes the signal format of the torque adjustment command to conform to the signal reception standard of the joint servo driver, ultimately generating a closed-loop trajectory control signal directly used to instruct the joint servo driver to adjust the output torque. After receiving this closed-loop trajectory control signal, the joint servo driver adjusts the output torque of the joint motor in real time, completing one closed-loop control cycle. Through continuous feedback and adjustment, the system ensures trajectory tracking accuracy and operational stability.

[0125] It should be understood that although the steps in the flowcharts of the embodiments described above are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the flowcharts of the embodiments described above may include multiple steps or multiple stages. These steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the steps or stages of other steps.

[0126] In one embodiment, this application also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the above-described method for tracking the trajectory of a dual-arm humanoid robot based on an improved terminal sliding mode algorithm.

[0127] In one embodiment, this application also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the above-described method for tracking the trajectory of a dual-arm humanoid robot based on an improved terminal sliding mode algorithm.

[0128] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of those different embodiments or examples.

[0129] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The device embodiments described above are merely illustrative. The components described as separate parts may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this disclosure according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0130] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any person skilled in the art can easily conceive of various variations or substitutions within the technical scope disclosed in this application, and these should all be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A trajectory tracking method for a dual-arm humanoid robot based on an improved terminal sliding mode algorithm, characterized in that, Includes the following steps: S1: Model the dual-arm robot system with flexible joints. Based on the Lagrange equation and the principle of motor-link separation, establish the augmented dynamic equation containing the dynamic coupling relationship between the motor and the link, and generate a state-space model containing flexible dynamic characteristics and system lumped disturbance terms. S2: Perform online vibration modal analysis on the state space model, linearize and decompose the state space model at the current system state and the desired trajectory, extract the dominant vibration frequency and the equivalent damping ratio, and generate real-time vibration characteristic parameters; S3: Adaptive construction of sliding mode surface for the real-time vibration characteristic parameters, adjusting the convergence gain according to the vibration frequency in the real-time vibration characteristic parameters and injecting nonlinear damping based on the damping ratio to generate a non-singular terminal sliding mode surface and sliding mode variable that are adaptive to frequency and damping. S4: Design a composite control law for the state space model, the non-singular terminal sliding surface, and the sliding variables. Combine the linear extended state observer to estimate and compensate the lumped disturbance term of the system. Estimate the lumped disturbance value online and feed it forward to compensate to the control law. Combine the equivalent control term derived based on Lyapunov stability to generate a continuous and smooth joint motor control torque. S5: Based on the execution feedback data of the joint motor control torque, perform closed-loop control processing, integrate the deviation between the current joint's actual position, speed and the desired trajectory, and generate a closed-loop trajectory control signal. The closed-loop trajectory control signal is used to instruct the joint motor to adjust the output torque.

2. The method according to claim 1, characterized in that, S1 includes: S11: Perform rigid-flexible coupling dynamic modeling on the rigid link and flexible joint components of the dual-arm robot system containing flexible joints, obtain the physical structure parameters, mass inertia properties and joint stiffness and damping characteristics of the dual-arm robot system, and derive the motor rotor dynamic equation and the link dynamic equation considering joint elasticity based on the Lagrange equation, respectively, to generate a set of coupled dynamic equations including position, velocity and joint elastic force. S12: Based on the coupled dynamic equations, the system state vector is augmented and the equations are first-order processed. By combining the motor position, the link position and their respective first derivatives into a new high-order state vector, an augmented state vector containing the position and velocity of the motor side and the link side is introduced. All the second-order differential equations in the coupled dynamic equations are rewritten as matrix differential equations with the augmented state vector and its first derivative as variables, generating the augmented state space equations of the dual-arm robot system. S13: Perform a structured decomposition on the augmented state space equation, explicitly decomposing the right-hand side of the augmented state space equation into nonlinear function terms that are only related to the state, input matrix terms that are related to the control input, and lumped terms that include unmodeled dynamics and external disturbances, generating a state space model that includes flexible dynamic characteristics and lumped disturbance terms. The state space model is used to indicate the exact mathematical model required for controller design and the sources of disturbances to be compensated.

3. The method according to claim 1, characterized in that, S2 includes: S21: Perform local linearization approximation on the state space model at the current operating point, using the actual system state and expected trajectory at the current moment as reference points, calculate the Jacobian matrix of the nonlinear dynamic function of the system with respect to the augmented state vector, and generate a linear time-varying system matrix characterizing the instantaneous dynamics of the system. S22: Perform eigenvalue analysis and modal decoupling on the linear time-varying system matrix, solve all eigenvalues ​​of the linear time-varying system matrix using numerical methods, and identify all conjugate complex root pairs from the complex eigenvalues. Each conjugate complex root pair corresponds to a vibration mode of the system, generating an eigenvalue set of the instantaneous vibration modes of the system. S23: Perform feature parameter quantization extraction for each vibration mode in the feature value set, calculate the damping ratio and undamped natural frequency of the vibration mode based on the real and imaginary parts of the conjugate complex root, screen out the dominant modes that have a significant impact on the dynamics of the system, classify and organize the frequency and damping ratio values ​​of the screened dominant modes, and generate real-time vibration feature parameters composed of the dominant vibration frequency feature quantity and the equivalent damping ratio feature quantity.

4. The method according to claim 1, characterized in that, S3 includes: S31: Based on the real-time vibration characteristic parameters, the dominant vibration frequency component characterizing the intensity of vibration of each joint and the equivalent damping ratio component characterizing its oscillation attenuation capability are respectively analyzed, and frequency characteristic quantities and damping characteristic quantities for independent adjustment and control behavior are generated. S32: Based on the frequency characteristic quantity and the damping characteristic quantity, perform online calculation of sliding mode surface gain and damping term; calculate the time-varying convergence gain corresponding to each joint for the dominant vibration frequency component through negative correlation function mapping, and construct a time-varying convergence gain diagonal matrix; and calculate the additional damping value required to compensate for the insufficient inherent damping of the system for the equivalent damping ratio component through compensation function, construct a time-varying damping injection diagonal matrix, and generate an adaptive gain matrix and damping matrix; S33: Combine the adaptive gain matrix, the damping matrix, and the system's position tracking error and error derivative obtained from sensor feedback and trajectory comparison processing according to the preset terminal attractor structure to form a complete sliding surface function expression. Substitute the current error state into the sliding surface function expression for calculation to generate a non-singular terminal sliding surface function with adaptive frequency and damping and its corresponding sliding variable value at the current moment.

5. The method according to claim 4, characterized in that, S4 includes: S41: Based on the state space model, the differential relationship of the non-singular terminal sliding surface, and the sliding variables, perform stability derivation of the equivalent control law, construct an energy function according to Lyapunov stability theory, set the derivative of the sliding variables to zero and ignore lumped disturbances, solve the control input required to maintain the ideal sliding mode from the state space model, and generate the equivalent control term that ensures the system trajectory approaches the sliding surface; S42: Design and perform online estimation of the lumped disturbance term defined in the state space model by expanding the lumped disturbance term into a new state, construct a linear observer with the system measurable as input, and configure the observer poles to quickly track the true value of the disturbance, thereby generating a real-time disturbance estimate of the lumped disturbance term. S43: Perform feedforward compensation synthesis and signal smoothing processing on the equivalent control term and the real-time disturbance estimate. After dynamically scaling the real-time disturbance estimate through the inverse of the adaptive gain matrix, feedforward superimpose it onto the equivalent control term. Then, replace the switching function with a continuous saturation function for the synthesized signal to eliminate high-frequency jitter and generate a continuous and smooth joint motor control torque.

6. The method according to any one of claims 1-5, characterized in that, S5 includes: S51: The execution process of the joint motor control torque is processed by collecting and fusing feedback data from multiple sources of sensors, and the actual position, actual speed and actual output torque of the motor measured by the joint encoder and current sensor are obtained in real time to generate an execution feedback dataset that reflects the instantaneous motion state of the system. S52: Obtain the expected position and expected velocity of each joint at the current moment from the expected trajectory planner, and perform corresponding component subtraction and weighted fusion with the actual position and actual velocity in the execution feedback dataset to generate a fused tracking deviation signal for closed-loop control decision-making; S53: Perform closed-loop control signal synthesis and output command generation processing on the fused tracking deviation signal, take the fused tracking deviation signal as the core feedback quantity, and map it into an adjustment command for the output torque of the joint motor according to the preset control logic, and generate a closed-loop trajectory control signal that is directly used to instruct the joint servo driver to adjust the output torque.

7. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the method of any one of claims 1 to 6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method of any one of claims 1 to 6.

Images