A planar link end effector switching control method and system based on bias and load state

Abstract

A kind of plane connecting rod end effector switching control method and system based on deviation and load state belong to motion adjustment control technical field.The method includes: obtaining target trajectory, current control mode, current target load level and safety threshold;Real-time acquisition of end execution point actual position, actual speed, drive joint state and drive load information;According to trajectory deviation, load state and abnormal duration determine current control state;In recoverable deviation state, execute first-level regulation, in protection control state, execute second-level regulation;When continuously meet recovery condition, restore autonomous tracking control mode, or execute autonomous tracking control mode with recovery target load level.The present application can improve trajectory stability, regulation continuity and operation safety margin of end effector in man-machine collaborative motion.

Description

Technical Field

[0001] This invention relates to the field of motion adjustment and control technology, specifically to a planar linkage end effector switching control method and system based on deviation and load state, which is particularly suitable for trajectory adjustment control of planar linkage end effectors during human-machine collaborative motion. Background Technology

[0002] Existing planar linkage end effectors typically track the target trajectory through position control, speed control, or load control when executing a preset trajectory. However, in actual operation, end effectors are susceptible to external interference, operator intervention, fluctuations in drive load, and accumulated trajectory deviations, leading to problems such as increased trajectory tracking errors, abnormal drive loads, and unstable control mode switching.

[0003] Existing solutions mostly focus on position tracking or load output under fixed control modes, or on the acquisition, display, and analysis of position and torque data. However, a unified control mechanism is lacking to address issues such as how the system can perform tiered adjustments based on real-time conditions when deviations increase or loads become abnormal, how to smoothly switch between different control modes, and how to restore the original control state after the anomaly is resolved.

[0004] Therefore, in the human-machine collaborative motion scenario of the planar linkage end effector, the following problems still exist: First, when the trajectory deviation gradually increases but has not yet reached a serious abnormality, there is a lack of a flexible and continuous first-level adjustment mechanism; Second, when the load state is abnormal or the deviation continues to deteriorate, there is a lack of a protective switching strategy corresponding to the real-time state; Third, after the abnormality is eliminated, there is a lack of a recovery control mechanism that can prevent frequent state jitter.

[0005] Therefore, it is necessary to propose a new switching control method that enables the planar linkage end effector to adjust the current control mode, target load level and trajectory adjustment parameters in stages according to real-time deviation and load status, and to restore to the autonomous tracking control state after the recovery conditions are met, so as to improve the trajectory stability, adjustment continuity and operational safety margin of the system. Summary of the Invention

[0006] Purpose of the invention

[0007] The purpose of this invention is to provide a planar linkage end effector switching control method and system based on deviation and load state, so as to solve the problems of insufficient trajectory stability, unstable state switching, and insufficient protection control of existing planar linkage end effectors under external intervention and load fluctuation conditions.

[0008] Technical solution

[0009] To achieve the above objectives, the present invention adopts the following technical solution:

[0010] A planar linkage end effector switching control method based on deviation and load state includes the following steps:

[0011] S1, obtain the target trajectory, current control mode, current target load level, and security threshold;

[0012] S2, during operation, collects the actual position, actual speed, drive joint status and drive load information of the end effector in real time;

[0013] S3, determine the trajectory deviation based on the target trajectory and the actual position and speed, determine the load status based on the drive load information, and determine the current control status as normal adjustment status, recoverable deviation status or protection control status based on the duration of the abnormality.

[0014] S4, when the current control state is normal adjustment state, maintain the current control mode, the current target load level and the current target speed;

[0015] S5, when the current control state is a recoverable deviation state, perform first-level adjustment on at least one parameter among the target load level, target speed, allowable deviation band and target trajectory range;

[0016] S6, when the current control state is protection control state, perform at least one of the following secondary adjustments: switch the current control mode to guide control mode; execute a protective guide trajectory; stop drive output;

[0017] S7. When the recovery conditions are met continuously, the current control mode is restored to the autonomous tracking control mode, or the autonomous tracking control mode is executed with the recovery target load level, wherein the recovery target load level is lower than the target load level before the anomaly occurred.

[0018] Furthermore, the trajectory deviation includes at least one or more of position deviation, velocity deviation, and trajectory completion deviation.

[0019] Furthermore, the drive load information includes at least one or more of drive torque and drive current.

[0020] Furthermore, when determining the current control state in step S3, at least two or three of the following are used: deviation threshold, load threshold, and abnormal duration threshold.

[0021] Furthermore, the first-level adjustment includes at least one of the following: reducing the target load level, reducing the target speed, widening the allowable deviation band, narrowing the target trajectory range, and reducing the target trajectory complexity.

[0022] Furthermore, the secondary adjustment includes at least one of the following: switching the current control mode to a guided control mode, executing a protective guided trajectory, and stopping the drive output.

[0023] Furthermore, the recovery conditions include: in continuous Within each sampling period, the trajectory deviation remained below the recovery deviation threshold, and the load status remained below the recovery load threshold.

[0024] Furthermore, after reverting to autonomous tracking control mode, if an anomaly occurs again within a preset suppression period, reverting to the target load level before the anomaly is prohibited.

[0025] Furthermore, after resuming to the autonomous tracking control mode, the target load level is first restored to a level lower than the target load level before the anomaly occurred, and the target load level is restored step by step after the stable operating conditions are met.

[0026] This invention also provides a planar linkage end effector control system, applying the aforementioned planar linkage end effector switching control method based on deviation and load state. The system includes a parameter acquisition module, a feedback acquisition module, a state determination module, a first-level adjustment module, a second-level adjustment module, and a recovery control module. The parameter acquisition module acquires the target trajectory, current control mode, current target load level, and safety threshold. The feedback acquisition module collects the actual position, actual speed, drive joint state, and drive load information of the end effector point in real time during operation. The state determination module determines the current control state based on trajectory deviation, load state, and abnormal duration. The first-level adjustment module performs first-level adjustment when the current control state is a recoverable deviation state. The second-level adjustment module performs second-level adjustment when the current control state is a protective control state. The recovery control module restores the current control mode to an autonomous tracking control mode when recovery conditions are continuously met, or performs autonomous tracking control mode at a restored target load level, wherein the restored target load level is lower than the target load level before the abnormality occurred.

[0027] Beneficial effects

[0028] 1. This invention can classify and determine the current control state based on trajectory deviation, load status, and abnormal duration, thereby improving the accuracy of state identification.

[0029] 2. This invention can form a graded response mechanism through primary and secondary regulation, thereby improving the adjustment continuity of the planar linkage end effector under conditions of increased deviation or abnormal load.

[0030] 3. The present invention can restore the autonomous tracking control mode after the recovery conditions are met, and use the recovery target load level for transition, thereby reducing the fluctuations caused by the control state switching.

[0031] 4. This invention is beneficial to improving the trajectory stability and operational safety margin of the end effector during human-machine collaborative motion.

[0032] 5. Compared with existing solutions that mainly focus on mode execution and data collection and analysis, this invention places greater emphasis on hierarchical adjustment and recovery switching control based on real-time status. Attached Figure Description

[0033] Figure 1 This is a general block diagram of the control system of the present invention;

[0034] Figure 2 This is a flowchart of the control state determination process of the present invention;

[0035] Figure 3 This is a flowchart of the first-level adjustment process of the present invention;

[0036] Figure 4 This is a flowchart of the two-stage adjustment process of the present invention;

[0037] Figure 5 This is the recovery control flowchart for the present invention;

[0038] Figure 6 This is a flowchart of the parameter update process for this invention. Detailed Implementation

[0039] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings. It should be understood that the following embodiments are for illustrative purposes only and are not intended to limit the scope of protection of the present invention. For those skilled in the art, corresponding modifications or substitutions can be made to the following embodiments without departing from the inventive concept, and all such modifications or substitutions should fall within the scope of protection defined by the claims of the present invention.

[0040] It should be noted that the following embodiments are used to describe the overall control flow and its components of the present invention step by step. The embodiments are not isolated from each other, but can be combined to form a complete switching control scheme. In some embodiments, the state determination module may use at least two of the following for joint determination: position deviation, speed deviation, trajectory completion deviation, driving torque, driving current, and abnormal duration.

[0041] Example 1: Overall Control Framework

[0042] like Figure 1 As shown, this embodiment provides a planar link end effector switching control method based on deviation and load state, which operates in a planar link end effector control system.

[0043] like Figure 1As shown, the control system includes a parameter acquisition module, a feedback acquisition module, a state determination module, a first-level adjustment module, a second-level adjustment module, and a recovery control module. The parameter acquisition module acquires the target trajectory, current control mode, current target load level, and safety threshold. The feedback acquisition module collects the actual position, actual speed, drive joint status, and drive load information of the end effector in real time during operation. The state determination module determines the current control state based on trajectory deviation, load status, and abnormal duration. The first-level adjustment module performs first-level adjustment when the current control state is a recoverable deviation state. The second-level adjustment module performs second-level adjustment when the current control state is a protective control state. The recovery control module restores the current control mode to an autonomous tracking control mode, or performs autonomous tracking control mode at a restored target load level, where the restored target load level is lower than the target load level before the abnormality occurred, when the recovery conditions are continuously met.

[0044] In this embodiment, the planar linkage end effector can use an existing planar multi-link mechanism as the actuation carrier, and the end actuation point can be the link end gripping part, the actuating end, or the human-machine interface end. To avoid duplication with existing structural disclosures, this embodiment does not focus on detailed limitations of the planar linkage mechanism body structure, but mainly revolves around the control logic.

[0045] Example 2: Trajectory Deviation Calculation

[0046] In this embodiment, the positional deviation is calculated using the following formula:

[0047] ;

[0048] in, This is for positional deviation; The x-coordinate of the target trajectory point; The ordinate of the target trajectory point; The actual x-coordinate of the final execution point; This represents the actual ordinate of the final execution point.

[0049] Speed ​​deviation is calculated using the following formula:

[0050] ;

[0051] in, For speed deviation; For target speed; This refers to the actual speed.

[0052] In some implementations, trajectory completion deviation can be further calculated. The trajectory completion deviation can be obtained from the difference or ratio between the actual completed trajectory length and the target trajectory length in the current cycle, and is used to help determine the trajectory tracking capability of the end effector under the current target load level.

[0053] Example 3: Load Status Calculation

[0054] In this embodiment, the load status is represented by a normalized load index:

[0055] ;

[0056] in, This is a normalized load metric; For real-time drive torque; The rated torque for driving the actuator.

[0057] When the drive current is used as load information, the load state can also be represented by the following formula:

[0058] ;

[0059] in, This is a normalized current load index; For real-time drive current; This is the rated drive current.

[0060] In some implementations, the status determination module may use normalized load metrics alone. A comprehensive approach can be used to make a judgment. and Make a joint judgment.

[0061] Example 4: Calculation of Abnormal Duration

[0062] In this embodiment, the duration of the anomaly is calculated by the number of consecutive anomaly sampling periods:

[0063] ;

[0064] in, The duration of the abnormality; This represents the number of consecutive abnormal samplings. The sampling period.

[0065] By introducing an abnormal duration, misjudgments caused by a single instantaneous disturbance can be avoided, thus improving the stability of state transitions.

[0066] Example 5: Control State Determination

[0067] like Figure 2As shown, in this embodiment, the current control state includes at least the normal adjustment state, the recoverable deviation state, and the protection control state.

[0068] like Figure 2 As shown, the control state determination can adopt the following rules:

[0069] ;

[0070] in, Current control status; This is a normal adjustment state; This is a recoverable deviation state; To protect the control status; The first deviation threshold; This is the second deviation threshold; The first load threshold; This is the second load threshold; To protect the control time threshold.

[0071] In this embodiment, when the trajectory deviation is small and the load is below the first load threshold, the end effector maintains normal adjustment; when the trajectory deviation is in the middle range or the load is in the middle range, it enters the recoverable deviation state; when the trajectory deviation reaches the high threshold, the load reaches the high threshold, or the abnormal duration exceeds the set time threshold, it enters the protection control state.

[0072] When some of the criteria for the recoverable deviation state and the protection control state are met at the same time, the protection control state is given priority.

[0073] Example 6: First-level regulation

[0074] like Figure 3 As shown, when the current control state is a recoverable deviation state, the system performs first-level adjustment instead of immediately stopping operation or switching to strong protection mode.

[0075] Level 1 adjustment includes at least one of the following: 1. reducing the target load level; 2. reducing the target speed; 3. widening the allowable deviation band; 4. narrowing the target trajectory range; 5. reducing the target trajectory complexity.

[0076] The target load level and the target load output can be related as follows:

[0077] ;

[0078] in, Output to the target load; The scaling factor corresponding to the target load level; This is the rated torque.

[0079] like Figure 3 As shown, after performing the first-level adjustment, the system continues to maintain the current control mode and continuously monitors the trajectory deviation and load status. If the deviation and load fall back to a safe range, the system can proceed to the recovery control decision; if the abnormality persists or worsens, the system will proceed to the second-level adjustment.

[0080] Example 7: Secondary Regulation

[0081] like Figure 4 As shown, when the current control state is protection control state, the system performs secondary regulation. Secondary regulation prioritizes ensuring system operational safety and prevents the end effector from continuing to maintain the original autonomous tracking control mode under high deviation and high load conditions.

[0082] Secondary adjustment includes at least one of the following: 1. Switching the current control mode to the guided control mode; 2. Executing a protective guided trajectory; 3. Stopping the drive output.

[0083] like Figure 4 As shown, the protective guidance trajectory can be set as a retraction trajectory from the current position of the end execution point to a preset safe position, or a guidance trajectory from the current position of the end execution point to the target trajectory safe zone.

[0084] When a linear guiding trajectory is used, it can be represented as:

[0085] ;

[0086] in, For the first The x-coordinate of each guide point; For the first The ordinates of the guide points; and These are the current coordinates of the end execution point; and These are the coordinates of the preset safe position; Number of segments for guiding the trajectory; .

[0087] Example 8: Restoring Control

[0088] like Figure 5 As shown, after performing first-level or second-level regulation, the system does not immediately return to the control state before the anomaly occurred, but first determines the recovery conditions.

[0089] The restoration criterion can take the following form:

[0090] ;

[0091] in, To restore the judgment result; To restore the number of consecutive sampling periods; To restore the deviation threshold; To restore the load threshold; For other situations.

[0092] like Figure 5 As shown, when When this occurs, the system allows recovery from boot control mode to autonomous tracking control mode, or execution of autonomous tracking control mode with a recovery target load level lower than the target load level before the anomaly occurred.

[0093] In this embodiment, after recovery, it is preferable to first run at a recovery target load level lower than the target load level before the anomaly occurred, and then gradually restore the original target load level after the stable operating conditions are met.

[0094] Example 9: Parameter Update

[0095] like Figure 6 As shown in this embodiment, after performing first-level or second-level adjustment, the system also records the corresponding abnormal state type, the time of abnormal occurrence, the adjustment type, and the recovery result, and updates the subsequent control parameters based on the recorded information.

[0096] The parameter update object includes at least one of the following: initial target load level, initial target speed, allowable deviation band, target trajectory range, and target trajectory complexity.

[0097] For example, if the system enters a recoverable deviation state multiple times in the same trajectory segment, the initial target load level corresponding to that trajectory segment will be automatically reduced in subsequent operating cycles; if the system maintains a normal adjustment state in multiple consecutive cycles, the target load level will be appropriately restored or the target trajectory range will be expanded in subsequent operating cycles.

[0098] like Figure 6 As shown, through the above method, the present invention forms a closed-loop regulation and control chain of "trajectory deviation and load status detection - control status determination - first-level adjustment - second-level adjustment - recovery control - parameter update".

[0099] The embodiments of the present invention have been described above with reference to the accompanying drawings, but the present invention is not limited to the specific embodiments described above. For those skilled in the art, any equivalent substitutions, modifications, or combinations made to the control state determination rules, threshold setting methods, first-level adjustment strategies, second-level adjustment strategies, and recovery control processes without departing from the technical concept of the present invention should be considered to fall within the protection scope defined by the claims of the present invention.

Claims

1. A planar linkage end effector switching control method based on deviation and load state, characterized in that, Includes the following steps: S1, obtain the target trajectory, current control mode, current target load level, and security threshold; S2, during operation, collects the actual position, actual speed, drive joint status and drive load information of the end effector in real time; S3, determine the trajectory deviation based on the target trajectory and the actual position and actual speed, determine the load state based on the drive load information, and determine the current control state as normal adjustment state, recoverable deviation state or protection control state based on the abnormal duration. S4, when the current control state is normal adjustment state, maintain the current control mode, the current target load level and the current target speed; S5, when the current control state is a recoverable deviation state, perform first-level adjustment on at least one parameter among the target load level, target speed, allowable deviation band and target trajectory range; S6, when the current control state is protection control state, perform at least one of the following secondary adjustments: switch the current control mode to guide control mode; execute a protective guide trajectory; stop drive output; S7. When the recovery conditions are met continuously, the current control mode is restored to the autonomous tracking control mode, or the autonomous tracking control mode is executed with the recovery target load level, wherein the recovery target load level is lower than the target load level before the anomaly occurred.

2. The planar linkage end effector switching control method based on deviation and load state according to claim 1, characterized in that: The trajectory deviation includes at least one or more of position deviation, speed deviation, and trajectory completion deviation; the drive load information includes at least one or more of drive torque and drive current.

3. The planar linkage end effector switching control method based on deviation and load state according to claim 1, characterized in that: In step S3, determining the current control state includes: when the trajectory deviation is less than a first deviation threshold and the load state is lower than a first load threshold, it is determined to be a normal adjustment state; when the trajectory deviation is greater than or equal to the first deviation threshold and less than a second deviation threshold, or the load state is greater than or equal to the first load threshold and less than the second load threshold, it is determined to be a recoverable deviation state; when the trajectory deviation is greater than or equal to the second deviation threshold, or the load state is greater than or equal to the second load threshold, or the abnormal duration is greater than a set time threshold, it is determined to be a protection control state.

4. The planar linkage end effector switching control method based on deviation and load state according to claim 1, characterized in that: The first-level adjustment in step S5 includes at least one of the following: reducing the target load level, reducing the target speed, widening the allowable deviation band, narrowing the target trajectory range, and reducing the target trajectory complexity.

5. The planar linkage end effector switching control method based on deviation and load state according to claim 1, characterized in that: The secondary adjustment in step S6 includes at least one of the following: switching the current control mode to the guide control mode; executing the protective guide trajectory; and stopping the drive output.

6. The planar linkage end effector switching control method based on deviation and load state according to claim 5, characterized in that: The protective guidance trajectory is a retraction trajectory from the current position of the end execution point to a preset safe position, or a guidance trajectory from the current position of the end execution point to the target trajectory safety zone.

7. The planar linkage end effector switching control method based on deviation and load state according to claim 1, characterized in that: The recovery conditions include: in continuous Within each sampling period, the trajectory deviation remains less than the recovery deviation threshold, and the load state remains lower than the recovery load threshold.

8. The planar linkage end effector switching control method based on deviation and load state according to claim 1, characterized in that: After resuming to autonomous tracking control mode, the system first implements control at a target load level lower than the target load level before the anomaly occurred, and then gradually restores the target load level after the stable operating conditions are met. If the anomaly occurs again within the preset suppression period, the system is prohibited from restoring to the target load level before the anomaly.

9. A planar linkage end effector control system, employing the planar linkage end effector switching control method based on deviation and load state as described in any one of claims 1 to 8, characterized in that, include: The parameter acquisition module is used to acquire the target trajectory, current control mode, current target load level, and safety threshold. The feedback acquisition module is used to collect the actual position, actual speed, drive joint status and drive load information of the end effector in real time during operation. The status determination module is used to determine the current control status based on trajectory deviation, load status, and duration of the anomaly; the first-level adjustment module is used to perform first-level adjustment when the current control status is a recoverable deviation status; the second-level adjustment module is used to perform second-level adjustment when the current control status is a protective control status; and the recovery control module is used to restore the current control mode to the autonomous tracking control mode, or to perform autonomous tracking control mode with a recovery target load level, wherein the recovery target load level is lower than the target load level before the anomaly occurred, when the recovery conditions are continuously met.

Images