Dynamic force tracking control method based on hybrid impedance and motor PID closed loop

Abstract

This invention discloses a dynamic force tracking control method based on hybrid impedance and motor PID closed-loop, applicable to the field of industrial robot control. This method involves constructing a stiffness-reduced mass-damped impedance model and an adaptive compensation rate. r ( t The invention employs a dual-loop dynamic force tracking control architecture, combining an outer-loop hybrid impedance control law with a bottom-level motor angle closed-loop PID controller to achieve dynamic force tracking control. This dual-loop control method enables precise, stable, and low-overshoot tracking control of dynamic contact force during flexible workpiece contact.

Description

Technical Field

[0001] This invention relates to the field of industrial robot control. More specifically, this invention relates to a dynamic force tracking control method based on hybrid impedance and motor PID closed-loop control. Background Technology

[0002] In industrial automated production processes, the end effectors of industrial robots often need to perform physical interactive operations on flexible layered workpieces with nonlinear contact characteristics, such as foam, rubber, silicone, composite films, soft encapsulation layers, gaskets, laminated cushioning materials, or other materials. These operations include contact holding, constant force pressing, compliant insertion, interface detection, surface scanning, or local pressing. These flexible workpieces typically exhibit significant nonlinear contact characteristics, and the stiffness and resistance distribution vary considerably between different material layers and different local areas. This can lead to complex operating conditions during contact operations, such as continuously increasing resistance, a sudden drop in resistance upon interface breakthrough, and local elastic recovery. These conditions can easily cause unstable contact force tracking, position control overshoot, workpiece surface damage, local indentation or scratches, decreased operational accuracy, and insufficient product consistency.

[0003] In existing technologies, traditional pure position control cannot adapt to changes in environmental stiffness, while standard impedance control can establish a dynamic relationship between force and position, but its impedance parameters are usually set offline and remain fixed. When encountering a sudden change in environmental resistance, the impedance model with fixed parameters often produces dynamic force tracking overshoot due to response lag, and may even cause excessive penetration and damage to the target object at the execution end. This makes it impossible for both methods to be applied to the complex application conditions of flexible workpieces.

[0004] Furthermore, in existing technologies, under the action of a step signal, traditional hybrid impedance is also used to achieve motor control, such as... Figure 3 As shown ( F e-1 This represents the actual contact force under traditional hybrid impedance control. F d-1 (Representing the expected contact force under traditional hybrid impedance control), but when using only hybrid impedance control, although the system can generate compliant displacement correction based on the contact force error, the lack of a closed-loop correction for the underlying motor angle results in weak matching between the system and the underlying motor actuator. The end effector's following accuracy for position correction commands is limited, manifesting as slow actual contact force convergence speed and poor dynamic response performance, making it difficult to meet the control requirements of high-precision flexible contact operations. In contrast, introducing a motor angle closed-loop into the hybrid impedance (such as...) Figure 4 As shown, F e-2 This represents the actual contact force under the motor angle closed-loop control method introduced in the mixed impedance. Fd-2 The system response speed and stability have both improved, indicating that the angle feedback of the underlying actuator can improve the end effector's ability to follow position correction commands, but the improvement effect is not satisfactory.

[0005] Under the action of the ramp signal Figure 6 The traditional hybrid impedance control shown exhibits some oscillation in the initial stage, indicating that its transient response performance is not ideal. However, as the control process continues, the response curve gradually converges, indicating that the scheme still has a certain force tracking capability under continuously varying reference force input. In contrast, after introducing a motor angle closed loop into the hybrid impedance (such as...), Figure 7 As shown in the figure, its force tracking effect is significantly improved, the response process is more stable, and the actual contact force can follow the change of the expected contact force better, but the overall tracking error is still somewhat lacking.

[0006] When the input signal is a sinusoidal signal Figure 9 The traditional hybrid impedance control unit shown in the diagram exhibits unsatisfactory tracking performance in the initial stage. Even after system adjustments, significant lag and oscillation persist, indicating limited ability to track periodic dynamic reference forces and insufficient capacity to meet stable force control requirements in complex contact environments. In contrast, introducing a motor angle closed-loop into the hybrid impedance (such as...) Figure 10 As shown in the figure, slight jitter may occur in the initial stage, but the subsequent tracking effect is significantly better than that of the single hybrid impedance control. However, although this scheme can complete the task of periodic contact force tracking, there is still a certain lag in the actual process of the contact force increasing from small to large.

[0007] The combined results of step, ramp, and sinusoidal signal responses show that the traditional hybrid impedance control scheme suffers from slow response speed, significant oscillation, and insufficient dynamic tracking capability under various input conditions. While the hybrid impedance and motor angle closed-loop control scheme, by introducing angle feedback, improves the end effector's ability to follow position correction commands and enhances system stability and force tracking, it still falls short in terms of overall tracking error and response speed. Summary of the Invention

[0008] One object of the present invention is to solve at least the above-mentioned problems and / or defects, and to provide at least the advantages described below.

[0009] To achieve these objectives and other advantages of the present invention, a dynamic force tracking control method based on hybrid impedance and motor PID closed-loop is provided, comprising: S1. After initializing the parameters of the industrial robot's control system and entering the control process, the system runs in a cyclical manner with a sampling period T, collecting the actual contact force through the force sensor on the end effector of the industrial robot. F e ( t ); S2, the control system will F e ( t ) and the built-in desired contact force F d ( t ) to compare and obtain t Contact force tracking error at any moment ; S3. Calculate the adaptive compensation rate for the current cycle using the following formula. ρ ( t ): In the above formula, T is the sampling period. ρ ( t - T ) represents the historical environmental compensation state quantity of the previous cycle, and σ represents the adaptive dynamic update rate at the current moment. F d ( t - T ) represents the expected contact force in the previous sampling period. F e ( t - T The actual contact force in the previous sampling period is denoted as ). S4, based on and ρ ( t The outer-loop hybrid impedance dynamic equation is constructed, and the relative position correction is obtained through further derivation. e ( t ); S5, based on e ( t Obtain the bottom-level angle following error of the motor at time t. e θ ( t ), by e θ ( t Substitute these values ​​into the inner-loop PID control equation to obtain the motor control command. U ( t This drives the end effector to complete the task.

[0010] Preferably, in S1, the parameter initialization includes: Set the desired contact forceF d Original reference position P d Target virtual quality parameters m d Basic virtual damping parameters b d The participation factor of σ in the adaptive dynamic update rate α , β and the system's anti-oscillation upper limit extreme value constant R limt Proportional gain of PID controller K p Integral gain K i Differential gain K d .

[0011] Preferably, in S3, the adaptive dynamic update rate σ is... and The absolute value is used as the joint feedforward input and is obtained by the following formula: In the above formula, Let be the rate of change of force error, and , This represents the contact force tracking error from the previous sampling period. for t The rate of change of force error at time t. α , β These represent the adjustment weights and participation factors corresponding to each parameter. R limt This represents the upper limit extreme value constant for the system's anti-oscillation.

[0012] Preferably, in S4, the outer-loop mixed impedance dynamic equation is characterized by the following equation: In the above formula, This is the system acceleration correction amount at the current moment. This is the system speed correction amount at the current moment. m d The preset target virtual quality parameters, b d Based on the virtual damping parameters.

[0013] Preferably, in S4, Obtained through the following formula: In the above formula, This is the system speed correction amount for the previous sampling period; based on The system velocity correction at the current moment is obtained recursively through numerical integration. : The relative position correction amount e ( t It is characterized by the following formula: In the above formula, This is the position correction amount for the previous sampling period.

[0014] The selected setting is to adjust the underlying angle following error in S5. e θ ( t The method to obtain ) is: S50, Based on relative position correction e ( t The following formula is used to generate a comprehensive position command that incorporates environmental mechanical feedback characteristics. P c ( t ): In the above formula, P d ( t (t) represents the original reference position preset by the system for the operation trajectory at time t; S51, Spatial Decoupling Transformation Function Based on Mechanical Mechanism Ф The following formula will be used to... P c ( t This translates into the target motor rotation angle required for the underlying drive shaft. θ c ( t ): In the above formula, L For the lead screw, i The gear ratio of the reducer; S52. Calculate the bottom angle following error using the following formula. e θ ( t ): In the above formula, θ e ( t The underlying servo driver uses an encoder to read the actual mechanical angle of the motor in real time.

[0015] Preferably, in S5, the inner-loop PID control equation is characterized by the following formula: In the above formula, K p , K i , K d These are the proportional gain, integral gain, and derivative gain of the PID controller, respectively. d This indicates that differentiation or integration operations are being performed.

[0016] The present invention has at least the following beneficial effects: Firstly, this invention uses a mass-damped impedance model with reduced stiffness as the basis for compliance control, which reduces the dependence on the assumption of stiffness in a fixed environment and is more suitable for working conditions where the local stiffness is unknown and the dynamic changes are obvious during the contact process of flexible workpieces.

[0017] Secondly, this invention introduces an adaptive compensation rate. ρ ( t The system updates in real time through discrete iteration, enabling it to compensate online for unknown stiffness changes, interlayer interface resistance changes, and local elastic release effects in flexible workpieces.

[0018] Third, the present invention constructs an adaptive dynamic update rate based on the combined force error and the force error change rate, which can automatically reduce the compensation increase when the contact force changes suddenly, thereby suppressing dynamic force overshoot and displacement overshoot.

[0019] Fourth, the present invention adopts a dual-loop structure of outer-loop hybrid impedance compliant control and inner-loop motor PID high-frequency closed-loop execution, which takes into account compliance, stability and engineering feasibility.

[0020] Other advantages, objectives and features of the present invention will become apparent in part from the following description, and in part from those skilled in the art through study and practice of the invention. Attached Figure Description

[0021] Figure 1 This is a schematic diagram of the closed-loop control system of hybrid impedance and PID controller in this invention; Figure 2 This is a schematic diagram of the algorithm processing flow of the present invention; Figure 3 This is the force tracking response curve when the input signal is a step signal, using the conventional hybrid impedance in existing technology; Figure 4 This is the force tracking response curve corresponding to the traditional hybrid impedance + motor closed-loop control in the existing technology when the input signal is a step signal; Figure 5The force tracking response curve corresponding to the hybrid impedance + motor PID closed-loop control of the present invention when the input signal is a step signal; Figure 6 This is the force tracking response curve when the input signal is a ramp signal and the conventional hybrid impedance in the existing technology is used; Figure 7 This is the force tracking response curve corresponding to the traditional hybrid impedance + motor closed-loop control in the existing technology when the input signal is a ramp signal; Figure 8 This is the force tracking response curve corresponding to the hybrid impedance + motor PID closed-loop control of the present invention when the input signal is a ramp signal; Figure 9 This is the force tracking response curve when the input signal is a sinusoidal signal and the conventional hybrid impedance in the existing technology is used; Figure 10 This is the force tracking response curve corresponding to the traditional hybrid impedance + motor closed-loop control in the existing technology when the input signal is a sinusoidal signal; Figure 11 This is the force tracking response curve corresponding to the hybrid impedance + motor PID closed-loop control of this invention when the input signal is a sinusoidal signal. Detailed Implementation

[0022] The present invention will now be described in further detail with reference to the accompanying drawings, so that those skilled in the art can implement it based on the description.

[0023] This invention provides a dynamic force tracking control method based on hybrid impedance and a motor PID closed loop. It combines an outer-loop hybrid impedance compliant control with an inner-loop motor PID controller closed loop to form a dual-loop dynamic force tracking control architecture. (In practical applications, the outer loop is responsible for outputting a compliant position correction based on real-time contact force changes, while the inner loop converts the position correction into a target rotation angle of the drive shaft after displacement fusion and angle mapping, and completes high-frequency servo execution of the motor through encoder feedback.) This achieves accurate, stable, and low-overshoot tracking control of the dynamic contact force during the contact, pressing, insertion, and detection processes of flexible workpieces. It should be noted that the hybrid impedance mentioned in this invention refers to the combination of a stiffened mass-damped impedance model and an adaptive compensation rate. ρ ( t The outer loop compliant control formed by the fusion is as follows: the former is used to establish the position compliant response relationship driven by force error, and the latter is used to compensate for unknown stiffness changes, interface resistance changes and local elastic release effects during the contact process of flexible workpieces online. In addition, low overshoot means that although the actual response of the system may exceed the target value in the transient stage during the process of dynamically tracking the desired contact force or target position, the overshoot is small.

[0024] like Figure 1 As shown, the robot system of the present invention obtains the desired contact force F d Actual contact force F e Starting from this point, the contact force tracking error is obtained through comparison. The outer loop impedance controller is based on The corresponding compliant displacement adjustment amount is calculated. This is used as the position compensation amount, and then the distance conversion in the outer loop hybrid impedance is used to convert the position compensation amount into the position correction amount. e This allows it to be compared with the original reference trajectory. P d Perform displacement fusion to form a comprehensive target position command. P c The target rotation angle of the motor is obtained through angle mapping. θ c The signal is then fed into the inner-loop PID controller; the PID controller generates control commands based on the deviation between the target rotation angle and the actual mechanical angle. U The drive motor drives the end effector to complete the operation. Driven by the motor system, the end effector undergoes actual movement, causing it to come into contact with the flexible workpiece or surrounding environment. Influenced by factors such as the stiffness of the flexible workpiece material, local deformation, interlayer resistance, and elastic recovery, a corresponding actual contact force response is generated. F e The force is detected in real time by a force sensor mounted on the end effector and fed back to the outer loop impedance controller as its input. It should be noted that... Figure 1 In θ e This is the actual mechanical angle, used to determine the target rotation angle of the motor. θ c The difference is used to obtain the underlying angle following error required by the inner-loop PID controller. e θ .

[0025] Specifically, such as Figure 2 As shown, its execution flow includes: Step 1: System Initialization and Data Acquisition Before starting the robot, initialize the system parameters and set the desired contact force. F d Original reference trajectory P d Target virtual mass parameters in impedance model m d Basic virtual damping parameters b d Error weighting factors in adaptive dynamic update rateα Error change rate weighting factor β and the upper limit constant for preventing divergence. R limt Simultaneously configure the proportional gain of the inner loop motor PID controller. K p Integral gain K i and differential gain K d .

[0026] After entering the control process, the system operates in a cycle with a sampling period T. The system uses a force sensor installed on the end effector to read the actual contact force in real time at a set sampling frequency. F e ( t ).

[0027] Step 2: Calculate the dynamic deviation and transient change characteristics of the contact force. In actual operation, the end effector reads the actual contact force applied by the environment through high-frequency sampling. F e ( t The system matches it with a pre-set desired contact force. F d ( t Real-time comparisons are performed, and the difference is calculated to obtain the contact force tracking error within the current control cycle. : In the above formula, t represents the current running time of the system. The contact force tracking error (also known as dynamic force error) is calculated at time t. F e ( t ) represents the actual contact force (i.e., environmental force) collected in real time by the end force sensor at time t. F d ( t ) represents the expected contact force preset by the system for a specific task at time t.

[0028] Contact force tracking error This signal directly reflects whether the force applied at the robot's end effector is too large or too small, and serves as the source signal for triggering all subsequent compliance compensation. To accurately capture the sudden drop in drag at the needle tip, the system combines historical errors stored in memory from the previous control cycle and calculates the rate of change of contact force error using the backward difference method. : In the above formula, t is the current running time of the system, and T is the sampling period of the system (i.e., the control step size). The rate of change of contact force error (i.e., the first-order differential derivative of the force error) characterizes the severity of the sudden change in force. This represents the contact force tracking error from the previous sampling period.

[0029] Step 3: Calculate the adaptive dynamic update rate σ to prevent overshoot. The absolute error calculated in step S2 absolute value of the rate of change of error As the joint feedforward input, substituting the preset decay update rate rule, the adaptive dynamic update rate σ at the current time is calculated: In the above formula, σ is the adaptive dynamic update rate calculated at the current time. α This is a weighting factor for adjusting the absolute value of the force error, used to adjust the system's sensitivity to the absolute error. β This is a weighting factor for adjusting the absolute value of the rate of change of force error, used to adjust the system's sensitivity to sudden changes. R limt This is the upper limit extreme value constant for system anti-oscillation, used to ensure that the denominator is not zero, prevent system divergence, and guarantee the absolute stability of the control system. In practical applications, it is used at the instant of sudden force change ( The denominator increases rapidly, causing σ to approach its minimum value instantaneously, thereby forcibly suppressing the violent burst of subsequent compensation, achieving a braking-like function at the microsecond level; while in the steady phase, σ automatically recovers to ensure steady-state following accuracy.

[0030] Step 4: Iteratively update the nonlinear adaptive compensation rate ρ ( t ) The real-time update rate σ calculated in step three is combined with the historical environmental compensation state quantity from the previous cycle. Based on historical force errors, the adaptive compensation rate for the current period is calculated using discrete iterative integration. ρ ( t ): In the above formula, ρ ( t The adaptive compensation rate is the adaptive compensation rate updated through adaptive iteration at the current moment. ρ ( t - T The value represents the historical adaptive compensation rate from the previous sampling period. F d ( t - T ) represents the expected contact force in the previous sampling period.F e ( t - T The actual contact force in the previous sampling period is ). b d These are the virtual foundation damping parameters preset in the impedance model.

[0031] Because this invention removes the stiffness term from the second-order impedance model to eliminate steady-state error, this adaptive compensation rate... ρ ( t In essence, it takes over the original static support function and can automatically output matching compensation force according to flexible materials with different unknown stiffness.

[0032] Step 5: Solve the stiffness-reducing impedance equation and output the macroscopic relative position correction. e ( t ) The contact force tracking error obtained in step two The adaptive compensation rate obtained from step four. ρ ( t Simultaneously, substituting this into the outer loop stiffness-reducing mass-damped impedance model, we establish the outer loop hybrid impedance dynamic equation under the current control cycle. (It should be noted that "hybrid" here refers to the contact force tracking error.) With adaptive compensation rate ρ ( t (The combined effect of) Because this invention addresses the characteristics of unknown local stiffness, significant changes in interlayer interface resistance, and abrupt changes in contact state during flexible workpiece contact operations, it removes the stiffness term from the traditional second-order impedance model, retaining only the virtual mass and virtual damping terms. This reduces the controller's dependence on the assumption of stiffness in a fixed environment and improves its compliance adaptability to complex contact conditions. Therefore, the outer-loop hybrid impedance dynamic equation can be expressed as: In the above formula, m d The target virtual mass parameter is preset in the impedance model. b d Based on the virtual damping parameters, This is the system acceleration correction calculated using the impedance equation at the current moment. The system velocity correction is obtained by integrating at the current moment; It should be noted that in the outer-loop mixed impedance dynamic equation, the target virtual mass parameter m d Virtual damping term is used to characterize the inertial response of a system to changes in force error. b d Adaptive compensation rate is used to suppress oscillations and improve the stability of dynamic contact processes. ρ ( t This is used to compensate for unknown local stiffness changes, interfacial resistance changes, and local elastic release effects during the contact process of flexible workpieces. The combined effect of these three factors enables the system to output smooth, stable, and environmentally adaptable position corrections based on the real-time contact status during the contact, pressing, insertion, and surface detection processes of flexible workpieces.

[0033] Furthermore, to accommodate the digital operations of the controller, the system acceleration correction at the current moment is first calculated using discretized differential operations. : In the above formula, The contact force tracking error at the current moment. This is the system speed correction amount for the previous sampling period; Then, through numerical integration, the system velocity correction at the current moment is recursively obtained. And the relative position correction amount that ultimately needs to be yielded or fed. e ( t (i.e., the macroscopic displacement that requires yielding or feeding at the end): In the above formula, This is the historical value of the position correction amount from the previous sampling period; In this step, the obtained position correction amount e ( t This is a macroscopic compliance correction relative to the original reference trajectory. Its physical meaning is: while keeping the overall predetermined process path unchanged, real-time, minute, and continuous compliance displacement adjustment of the end effector is performed based on the current contact state. This position correction will be superimposed on the preset absolute working trajectory in subsequent steps to generate a comprehensive target position command that integrates mechanical feedback information, which will then be used by the underlying actuator to further complete spatial mapping and motor servo control.

[0034] Step Six: Secure Fusion of Force-Position Coupling Integrated Commands The relative position correction calculated in step five e ( t ) superimposed onto the original desired reference position pre-set by the system for the operation trajectory P d ( t On the above, a comprehensive position command incorporating environmental mechanical feedback characteristics is generated. P c ( t ): In the above formula, P c ( t The command at time t is the integrated target position command that incorporates mechanical feedback compensation. P d ( t ( ) represents the original expected reference position preset by the system for the operation trajectory at time t. It should be noted that when e ( t When )>0, it indicates that the position moves forward. e ( t When ) < 0, it indicates that the position has moved backward. In this step, the position correction amount with a direction sign is calculated by the outer loop mixed impedance. e ( t ), merged into the original reference position P d ( t This leads to the formation of a comprehensive target location that integrates environmental mechanical feedback and is then used for actual deployment. P c ( t ).

[0035] Step 7, Spatial Decoupling Transformation (Position to Angle) Considering that the actual robot's driving source is a rotary motor, the above-mentioned integrated target position command after incorporating environmental mechanical feedback... P c ( t Kinematic decoupling of the mechanism is necessary. This can be achieved by introducing the transmission ratio equation or inverse kinematic mapping function of the mechanical system. Ф Transforming spatial position into the target motor rotation angle required for the underlying drive shaft. θ c ( t ): In the above formula, θ c ( t Let t be the target rotation angle that the underlying servo motor needs to execute at time t. This represents the spatial decoupling transformation function for mechanical mechanisms.

[0036] The mechanism kinematic mapping function In this embodiment, the displacement-angle conversion function is specifically established based on the reducer transmission ratio and the lead screw, namely: In the above formula, L is the lead of the lead screw. i This represents the gear ratio of the reducer.

[0037] Step 8: Inner ring motor PID high-frequency braking The underlying servo driver uses a high-precision encoder to read the actual mechanical angle of the motor in real time. θ e ( t The difference between the base angle and the target angle is calculated using the following formula to determine the bottom-level angle following error. e θ ( t ): The control system calls the inner-loop PID controller operating in the extremely high-frequency range to output the final motor control command. U ( t ): In the above formula, U ( t ( ) represents the control command output by the inner-loop PID controller to the underlying motor driver at time t. θ e ( t ( ) represents the actual mechanical angle of the motor at time t. e θ ( t Let be the bottom-level angle following error of the motor at time t, which is defined as the difference between the target angle and the actual angle, i.e. , K p The proportional gain of the inner-loop servo PID controller. K i The integral gain of the inner-loop servo PID controller. K d The derivative gain of the inner-loop servo PID controller; Will U ( t The signal is sent to the motor actuator via the drive circuit, forcing the motor to overcome the backlash and internal static friction of the reducer and move precisely.

[0038] At this point, the single-cycle control from the "low-frequency compliant outer loop" to the "high-frequency rigid inner loop" has completely ended, and it awaits the arrival of the next sampling period T to return to step one and repeat the cycle. Figure 5As shown, under the action of a step signal, the hybrid impedance and motor angle closed-loop PID control of this invention exhibits superior performance in terms of response speed, overshoot suppression, and steady-state tracking. This scheme, based on the compliant displacement adjustment achieved by the outer-loop hybrid impedance control, uses the inner-loop PID to correct the motor angle error in real time, enabling the drive motor to more accurately follow the target angle command. This improves the end-effector displacement execution accuracy, shortens the force response time, and reduces contact force overshoot. Although the system may experience slight fluctuations in the steady-state phase, the overall response speed is faster, the force tracking error is smaller, and the comprehensive control performance is superior to the existing traditional hybrid impedance control and hybrid impedance and motor angle closed-loop control schemes.

[0039] Therefore, based solely on the step signal response results, the hybrid impedance and motor angle closed-loop PID control scheme is more suitable for dynamic contact operations of flexible workpieces. This scheme can balance the compliant control of the outer loop hybrid impedance with the high-precision execution control of the inner loop, improving dynamic force tracking stability and control robustness while ensuring contact safety. It provides a more reliable control method for scenarios such as contact, pressing, insertion, and surface detection of flexible workpieces.

[0040] like Figure 8 As shown, when the input signal is a ramp signal, the hybrid impedance and motor angle closed-loop PID control of this invention also exhibits good force tracking performance with a smaller overall tracking error. Although brief jitter may occur in the initial stage, and slight abrupt changes may exist in the intermediate local area, the system can recover stability quickly, and the actual contact force can still effectively follow the ramp reference signal. Compared with the previous two schemes, the hybrid impedance and motor angle closed-loop PID control performs better in terms of dynamic tracking accuracy and disturbance rejection recovery capability, and is more suitable for continuous contact force tracking scenarios such as flexible workpiece contact, pressing, insertion, and surface detection.

[0041] like Figure 11 As shown, when the input signal is a sinusoidal signal, the hybrid impedance and motor angle closed-loop PID control of the present invention can achieve a faster recovery and stability, even though slight oscillations may occur at local troughs. The overall lag is smaller and the dynamic tracking capability is stronger.

[0042] The combined results of step, ramp, and sinusoidal signal responses show that the hybrid impedance and motor angle closed-loop PID control scheme of this invention adds PID correction to the angle closed loop, which can further reduce response lag, improve dynamic tracking accuracy, and enhance the system's adaptability to different reference force inputs.

[0043] Therefore, when faced with dynamic reference force inputs of different forms, such as step, ramp, and sinusoidal forces, the hybrid impedance and motor angle closed-loop PID control scheme of this invention exhibits superior overall performance, with faster response speed, smaller steady-state error, better overshoot suppression capability, and stronger robustness. In summary, for dynamic contact operation scenarios such as flexible workpiece contact, pressing, insertion, and surface detection, this invention is more suitable as the preferred solution for subsequent system verification and engineering implementation.

[0044] The above solution is merely an illustration of a preferred example and is not limited thereto. When implementing this invention, appropriate substitutions and / or modifications can be made according to the user's needs.

[0045] Although embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. It can be applied to various fields suitable for the present invention. Other modifications can be readily made by those skilled in the art. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details and examples shown and described herein.

Claims

1. A dynamic force tracking control method based on hybrid impedance and motor PID closed loop, characterized in that, include: S1. After initializing the parameters of the industrial robot's control system and entering the control process, the system runs in a cyclical manner with a sampling period T, collecting the actual contact force through the force sensor on the end effector of the industrial robot. F e ( t ); S2, the control system will F e ( t ) and the built-in desired contact force F d ( t ) to compare and obtain t Contact force tracking error at any moment ; S3. Calculate the adaptive compensation rate for the current cycle using the following formula. ρ ( t ): In the above formula, T is the sampling period. ρ ( t - T ) represents the historical environmental compensation state quantity of the previous cycle, and σ represents the adaptive dynamic update rate at the current moment. F d ( t - T () represents the expected contact force in the previous sampling period. F e ( t - T The actual contact force in the previous sampling period is denoted as ). S4, based on and ρ ( t The outer-loop hybrid impedance dynamic equation is constructed, and the relative position correction is obtained through further derivation. e ( t ); S5, based on e ( t Obtain the bottom-level angle following error of the motor at time t. e θ ( t ), by e θ ( t Substitute these values ​​into the inner-loop PID control equation to obtain the motor control command. U ( t This drives the end effector to complete the task.

2. The dynamic force tracking control method based on hybrid impedance and motor PID closed loop as described in claim 1, characterized in that, In S1, the parameter initialization includes: Set the desired contact force F d Original reference position P d Target virtual mass parameters m d Basic virtual damping parameters b d The participation factor of σ in the adaptive dynamic update rate α , β and the system's anti-oscillation upper limit extreme value constant R limt Proportional gain of PID controller K p Integral gain K i Differential gain K d .

3. The dynamic force tracking control method based on hybrid impedance and motor PID closed loop as described in claim 1, characterized in that, In S3, the adaptive dynamic update rate σ is... and The absolute value is used as the joint feedforward input and is obtained by the following formula: In the above formula, Let be the rate of change of force error, and , This represents the contact force tracking error from the previous sampling period. for t The rate of change of force error at time t. α , β These represent the adjustment weights and participation factors corresponding to each parameter. R limt This represents the upper limit extreme value constant for the system's anti-oscillation.

4. The dynamic force tracking control method based on hybrid impedance and motor PID closed loop as described in claim 1, characterized in that, In S4, the outer-loop mixed impedance dynamic equation is characterized by the following equation: In the above formula, This is the system acceleration correction amount at the current moment. This is the system speed correction amount at the current moment. m d The preset target virtual quality parameters, b d Based on the virtual damping parameters.

5. The dynamic force tracking control method based on hybrid impedance and motor PID closed loop as described in claim 4, characterized in that, In S4, Obtained through the following formula: In the above formula, This is the system speed correction amount for the previous sampling period; based on The system velocity correction at the current moment is obtained recursively through numerical integration. : The relative position correction amount e ( t It is characterized by the following formula: In the above formula, This is the position correction amount for the previous sampling period.

6. The dynamic force tracking control method based on hybrid impedance and motor PID closed loop as described in claim 1, characterized in that, In S5, the underlying angle following error e θ ( t The method to obtain ) is: S50, Based on relative position correction e ( t The following formula is used to generate a comprehensive position command that incorporates environmental mechanical feedback characteristics. P c ( t ): In the above formula, P d ( t (t) represents the original reference position preset by the system for the operation trajectory at time t; S51, Spatial Decoupling Transformation Function Based on Mechanical Mechanism Ф The following formula will be used to... P c ( t This translates into the target motor rotation angle required for the underlying drive shaft. θ c ( t ): In the above formula, L For the lead screw, i The gear ratio of the reducer; S52. Calculate the bottom angle following error using the following formula. e θ ( t ): In the above formula, θ e ( t The underlying servo driver uses an encoder to read the actual mechanical angle of the motor in real time.

7. The dynamic force tracking control method based on hybrid impedance and motor PID closed loop as described in claim 1, characterized in that, In S5, the inner-loop PID control equation is characterized by the following formula: In the above formula, K p , K i , K d These are the proportional gain, integral gain, and derivative gain of the PID controller, respectively. d This indicates that differentiation or integration operations are being performed.

Images