A Dexterous Hand Compliant Protection Method and System Based on Drive Current Criterion
By introducing a compliant protection method based on drive current criteria into the dexterous hand drive system, and using the pre-protection drive output and the direction normalized current evaluation quantity for joint judgment, compliant protection of the dexterous hand under abnormal load is realized. This solves the problems of unreliable protection and large recovery impact in the prior art, reduces costs and improves adaptability.
Patent Information
- Application Number
- CN202611629161.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-21
- Publication Date
- 2026-11-17
AI Technical Summary
Existing technologies in dexterous hand drive systems suffer from unreliable protection, large recovery impact, and high cost due to the lack of motor-end encoders, backlash and high back electromotive force in joint-end feedback. In particular, it is difficult to achieve compliant protection under abnormal contact or overload conditions.
A compliant protection method based on drive current criterion is adopted. By setting a compliant protection loop in the motor drive system, the drive output before protection and the direction normalized current evaluation quantity are used for joint judgment to achieve limited drive amplitude, internal state synchronization and gradual recovery, avoid dependence on high bandwidth current closed loop and reduce the probability of false protection.
It effectively solves the problem of compliant protection for dexterous hands under abnormal loads, reduces recovery shock, lowers costs, improves the reliability and adaptability of protection, and meets the independent configuration requirements of multi-degree-of-freedom dexterous hands.
Smart Images

Figure CN123103718A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of robot dexterous hand control technology, and in particular relates to a dexterous hand compliance protection method and system based on drive current criterion. Background Technology
[0002] Dexterous hands are core end effectors in the fields of humanoid robots and prosthetics, typically integrated into robotic arms, humanoid robots, or prosthetic terminals to grasp, manipulate, and release objects. Dexterous hands operate in several scenarios requiring protection: during grasping operations, the contact force must be controlled after the fingers touch the object to prevent damage; in position control mode, if the fingers accidentally collide with obstacles or encounter hard barriers, impact mitigation is necessary; in cases of motion planning failure, positioning deviation, or obstacle avoidance failure, the fingers may collide with the external environment at high speeds, requiring rapid energy absorption and protection from rigid impacts. All of these scenarios necessitate protecting the motors, actuators, and transmission mechanisms while minimizing damage to the manipulated object under abnormal contact or continuous load conditions.
[0003] Therefore, dexterous hands typically incorporate multiple degrees of freedom within a finite volume. Each degree of freedom includes a motor, a reduction gear mechanism, a joint mechanism, joint position detection elements, and a drive control circuit. To balance size, weight, and response speed, some dexterous hands use a coreless DC brushed motor as the drive source and incorporate encoders at the joint ends to provide position feedback.
[0004] However, the above structure faces the following specific technical constraints in practical applications: (1) This type of dexterous hand usually only has an encoder at the joint end and no encoder at the motor end. Since there is a reduction mechanism and transmission backlash between the motor and the encoder at the joint end, the position and speed at the joint end cannot accurately reflect the transient state of the motor rotor. Therefore, the stall / overload protection method that relies on the motor end speed, rotor position or stable current closed loop is difficult to apply directly.
[0005] (2) Hollow cup DC brushed motors have the characteristics of small rotor moment of inertia and small inductance, and also have a high back electromotive force coefficient per unit volume. The relationship between its voltage, current, and speed can be approximated as: ; in, This is the motor terminal voltage. For winding current, For winding resistance, For winding inductance, The back electromotive force coefficient, This refers to the motor speed. At higher levels, the back electromotive force It will occupy a large voltage margin, reducing the current regulation space; under the conditions of small inductance (low inductance value of winding inductance) and limited drive voltage (low motor terminal voltage), high bandwidth current closed loop is prone to saturation and oscillation.
[0006] (3) When the dexterous hand grasps an object or comes into contact with the external environment, if the finger is blocked by a rigid object or collides with an external hard object at a certain speed due to positioning deviation / obstacle avoidance failure, the position error may persist for a long time. The position controller will continue to give a large driving demand, resulting in increased motor current, increased heat generation, and increased transmission impact.
[0007] In existing technologies, one type of DC brushed motor stall protection scheme employs current tracking (e.g., a DC brushed motor stall protection device and method based on current tracking disclosed in Chinese Patent No. CN104242246B). This scheme primarily determines stall based on current change trends and stops the motor after stalling. It does not involve jointly determining whether to use the pre-protection drive output as a control intent criterion or the direction-normalized current evaluation quantity as a load response criterion, nor does it address limited drive compliance protection, controller internal state synchronization, gradual recovery, or reentrant protection mechanisms. If this type of scheme is applied to the driving scenarios of dexterous hands or robot finger joints, the following technical drawbacks exist: (1) The stall judgment of this type of scheme relies on the current tracking algorithm to identify the current change trend, which requires the current closed-loop control to have a certain degree of stability and predictability. However, in the dexterous hand scenario, due to the high back electromotive force of the hollow cup DC brushed motor and the limited current adjustment margin, the high bandwidth current closed loop itself is difficult to establish stably, so the current tracking judgment premise of this type of scheme is difficult to meet.
[0008] (2) This type of scheme takes the protection method of stopping the machine or stopping the motor after judging the stall, which is a hard cut-off protection. Once this protection method is triggered, the driving force is completely lost, which cannot meet the requirements of smooth unloading, maintaining limited controllable driving capability and task continuity required by the dexterous hand in the grasping process.
[0009] (3) This type of scheme does not consider the working conditions of no encoder at the motor end, only encoder at the joint end and backlash in the transmission. Its judgment is mainly based on the current itself, does not include the driving intention before protection in the judgment criteria, and does not involve the processing method of synchronizing the internal state to the protection working point after protection is triggered. Therefore, it is difficult to smoothly switch back to normal closed-loop control after protection is triggered, and recovery shock is likely to occur.
[0010] (4) This type of scheme does not disclose the smooth recovery mechanism and reentrant protection mechanism after the protection is released, that is, when the stall or overload conditions are released, how the driving capability gradually recovers from the restricted state to the normal state, and how to re-enter the protection state if it encounters obstacles again during the recovery process.
[0011] (5) Simple current threshold judgment is easily affected by starting shock, commutation transient, current spike and sampling noise, and the probability of false protection is high.
[0012] In summary, this type of solution cannot solve the problem of providing compliant protection and smooth recovery for abnormal contact or overload conditions in dexterous hand drive systems under conditions such as no encoder at the motor end, backlash in the joint feedback, and instability of the current closed loop due to high back EMF. Furthermore, the simple current threshold judgment is easily affected by starting shocks, commutation transients, current spikes, and sampling noise, resulting in a high probability of false protection. Summary of the Invention
[0013] The technical problem this invention aims to solve is: addressing the issues of unreliable protection, large recovery impact, and high cost in existing technologies due to reliance on high-bandwidth current closed-loop systems, dependence on motor-end rotor status, hard-cut-off protection, difficulty in distinguishing between instantaneous spikes and continuous anomalies, and the need for additional sensors. This invention provides a compliant, dexterous protection method and system based on drive current criterion. This method utilizes existing current sampling, joint-end encoders, and drive output information to reduce the actual drive capability when an abnormal load risk is detected, and gradually restores the drive capability during the protection release process.
[0014] To address the aforementioned technical problems, the present invention provides the following technical solution: In the first aspect, the present invention provides a compliant protection method for dexterous hands based on drive current criteria. The basic idea is to set up a compliant protection link between the original control output and the motor drive, take the drive output before protection as the control intention criterion, take the normalized current evaluation quantity formed by the motor current as the load response criterion, and achieve compliant protection of the degree of freedom of dexterous hands through joint judgment, continuous confirmation, limited drive amplitude, internal state synchronization and gradual recovery.
[0015] Among them, the pre-protection drive output refers to the drive output that has completed position control, feedforward, compensation, and filtering processes but has not yet undergone compliance protection processing, denoted as... The actual drive output after protection refers to the actuated quantity sent to the motor drive unit after compliant protection processing, denoted as... .
[0016] The dexterity hand compliance protection method periodically performs the following steps: Step S1: Obtain the pre-protection drive output And collect the current sampling value of the motor drive circuit. ; Step S2: Based on the protection pre-drive output The driving direction affects the current sampling value Direction normalization is performed to obtain the direction-normalized current evaluation quantity. ; Step S3: Determine the output of the protection pre-drive. and the direction-normalized current evaluation quantity Check if a positive or negative anomaly condition is met; if the positive anomaly condition is met, increment the positive continuous confirmation count and clear the negative continuous confirmation count; if the negative anomaly condition is met, increment the negative continuous confirmation count and clear the positive continuous confirmation count; if neither the positive nor the negative anomaly condition is met, clear both the positive and negative continuous confirmation counts; determine if the continuous confirmation count in the same direction has reached a preset number; if the continuous confirmation count in the same direction has reached a preset number, generate a compliant protection trigger signal; if the preset number has not been reached, continue to the next control cycle; Step S4: In response to the compliant protection trigger signal, output the actual drive after protection. Limit to protection limit value Internally, it synchronizes the internal state with the actual drive output after protection. Corresponding protection working points; Step S5: If the compliant protection trigger signal is not generated again after the protection duration ends, the actual drive output after protection is gradually increased according to the progressive recovery function. The allowed output range; Step S6: During the execution of steps S4 and S5, continue to execute steps S1 to S3. If the abnormal condition in the same direction is met again and the preset number of times is reached, then re-enter step S4.
[0017] In one implementation, step S2, the direction normalization includes: Take the current sampling value Amplitude: ; According to the protection pre-drive output The sign of the current yields the direction-normalized current evaluation quantity: ; in, For a sign function, when When, take 1, when When, take -1; in one implementation, when When, take 0, at this time The positive and negative anomaly conditions are not met, therefore compliant protection is not triggered. Used to characterize the load state under the current driving direction.
[0018] In one implementation, step S3 includes the following abnormal conditions: Positive outlier conditions are: ; The reverse anomaly condition is: ; in, The current evaluation threshold is expressed in mA. To protect the front-drive output threshold, the unit is mV; the positive anomaly condition and the negative anomaly condition share the same... and the same .
[0019] In one implementation, step S3, the continuous confirmation count includes: setting a positive continuous confirmation count and a negative continuous confirmation count; when the positive abnormal condition is met, increasing the positive continuous confirmation count and clearing the negative continuous confirmation count; when the negative abnormal condition is met, increasing the negative continuous confirmation count and clearing the positive continuous confirmation count; when neither the positive nor the negative abnormal condition is met, clearing the positive and negative continuous confirmation counts; performing continuous confirmation by combining the directional normalized current evaluation value of the current control cycle and the directional normalized current evaluation value of the previous control cycle; generating the compliant protection trigger signal only when the continuous confirmation count in the same direction reaches a preset number.
[0020] In one implementation, continuity confirmation is performed by combining the directional normalized current evaluation value of the current control cycle and the directional normalized current evaluation value of the previous control cycle, including: obtaining the directional normalized current evaluation value of the previous control cycle. ; when the direction-normalized current evaluation quantity of the current control cycle and the previous control cycle All exceed the current evaluation threshold corresponding to the same direction. Furthermore, when the current protection drive output meets the drive output threshold condition for the corresponding direction, the continuous confirmation count for the corresponding direction is increased.
[0021] In one implementation, in step S4, the actual drive output after protection is... Limit to protection limit value Includes: when hour, ; when hour, ; when hour, ; in, To protect the amplitude limit, the unit is mV.
[0022] In one implementation, in step S4, the protection operating point is the control output operating point corresponding to the actual drive output after protection, so as to synchronize the operating points; the internal state synchronization includes at least one of the following: clearing or limiting the integral state, updating or synchronizing the internal state of the dynamic filter or notch filter to the state corresponding to the actual drive output after protection, and updating the current control output and historical output buffer.
[0023] In one implementation, step S5 includes the gradual recovery function comprising: setting a normalized recovery progress. The smoothing factor changes from 0 to 1 as follows: ; The actual drive output after protection is gradually increased according to the smoothing factor. The upper and lower limits of the allowed output are set so that the upper and lower limits of the allowed output are gradually restored from the protection limit range to the normal driving range.
[0024] In one implementation, the asymptotic recovery function is a linear function, an exponential function, a sigmoid function, or other continuous asymptotic function.
[0025] In one implementation, when the normalized recovery progress When the progress reaches 1, the recovery is considered complete, and the system returns to normal control. The normalized recovery progress is then updated. Reset.
[0026] In one implementation, when the abnormal conditions are met consecutively again during the recovery phase, the system re-enters a compliant protection state with limited drive capability, and the normalized recovery progress is adjusted. Reset to zero and restart the calculation of protection duration.
[0027] In one embodiment, the method further includes: independently setting at least one of the following for multiple degrees of freedom of the dexterous hand: current evaluation threshold, protection pre-drive output threshold, number of consecutive confirmations, protection limit value, protection duration, and recovery time; each degree of freedom maintains its own consecutive confirmation count, protection status, and recovery status.
[0028] In one embodiment, the method is applied to a dexterous hand, wherein the motor of the dexterous hand is a hollow-cup DC brushed motor, the motor end of the dexterous hand does not have an encoder, but the joint end has an encoder, and a reduction gear transmission mechanism with backlash is provided between the motor and the joint end; the method is based on the protection pre-drive output. and the current sampling value Perform abnormal condition judgment.
[0029] Secondly, the present invention also provides a dexterous hand compliance protection system based on a drive current criterion, comprising: The control output calculation unit is used to output the protection pre-drive output. The protection pre-drive output The drive output after position control, feedforward, compensation and filtering have not yet undergone compliance protection processing; The current sampling unit is used to collect the current sampling values of the motor drive circuit. ; The compliant protection unit includes a direction normalization module, a joint judgment module, a continuous confirmation module, a limiting protection module, a state synchronization module, and a gradual recovery module; the direction normalization module is used to output the protection pre-drive based on the... The driving direction affects the current sampling value Direction normalization is performed to obtain the direction-normalized current evaluation quantity. The joint judgment module is used to determine the output of the protection pre-drive based on the... and the direction-normalized current evaluation quantity The system determines whether an abnormal condition is positive or negative. The continuous confirmation module increments the positive continuous confirmation count and resets the negative continuous confirmation count when the positive abnormal condition is met, increments the negative continuous confirmation count and resets the positive continuous confirmation count when the negative abnormal condition is met, and clears both the positive and negative continuous confirmation counts when neither condition is met. It also determines whether the continuous confirmation count in the same direction has reached a preset number; if so, it generates a compliant protection trigger signal. The limiting protection module responds to the compliant protection trigger signal by adjusting the actual drive output after protection. Limit to protection limit value The state synchronization module is used to synchronize the internal state of the control output calculation unit with the actual drive output after protection. The corresponding protection operating point; the progressive recovery module is used to gradually increase the actual drive output after protection according to the progressive recovery function if the compliant protection trigger signal is not generated again after the protection duration ends. The allowed output range; The drive output unit is used to output the actual drive after the protection is applied. Drive motor and reduction gear transmission mechanism; A joint position acquisition unit is used to acquire joint position information and feed it back to the control output calculation unit; During the limiting and gradual recovery process, the continuous confirmation module continuously adjusts the output based on the protection pre-drive output. and the current sampling value An abnormal condition is determined. If the same abnormal condition is met again and the number of consecutive abnormal conditions reaches a preset number, the compliant protection trigger signal is generated, and the amplitude limiting protection module re-enters the protection state.
[0030] Thirdly, the present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the dexterity hand compliance protection method as described in the first aspect.
[0031] Fourthly, the present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the dexterity hand compliance protection method as described in the first aspect.
[0032] Compared with the prior art, the advantages of the present invention are as follows: (1) Corresponding to the defects of high bandwidth current closed loop being prone to saturation or oscillation: This invention transforms the current from "precise closed loop tracking quantity" to "load state response criterion", and makes a joint judgment in combination with the control intention represented by the protection pre-drive output, avoiding dependence on the stability of high bandwidth current closed loop, and effectively solving the problem that the current closed loop cannot be stably established due to the high back electromotive force, small voltage margin and small inductance of the hollow cup motor.
[0033] (2) The method that depends on the speed of the motor end / rotor state cannot be implemented: The present invention performs directional normalization processing on the current sampling quantity according to the driving direction, so that the directional normalized current evaluation quantity can represent the load state under the current driving direction; abnormal load can be identified without the need for the motor end encoder or rotor position information, which effectively solves the problem that abnormal load is difficult to judge when there is no encoder at the motor end or there is transmission backlash in the joint end feedback.
[0034] (3) Corresponding to the stiff protection action caused by shutdown or hard cut-off: After the protection is triggered, the present invention enters a compliant protection state with limited driving capability, limits the actual driving output instead of completely cutting it off, and gradually expands the allowable output range through the progressive recovery function, which effectively solves the problems of stiff protection action, task interruption and large recovery impact in traditional protection.
[0035] (4) The defect of easy false triggering of simple current threshold: The present invention uses the current-drive output joint judgment and continuous multi-cycle confirmation mechanism to require the protection to be triggered only after the abnormal conditions in the same direction are met for a preset number of times. This effectively distinguishes between instantaneous current spikes and continuous abnormal loads, and solves the problem that starting impact, commutation transients and sampling noise are misjudged as abnormal loads.
[0036] (5) The present invention mainly reuses existing current sampling, joint end encoder and PWM drive hardware resources, and achieves compliant protection through software state machine. It does not require additional force sensor, tactile array or mechanical buffer mechanism, and effectively solves the problems of increased volume, increased cost, complex wiring harness and difficult assembly.
[0037] (6) Defects in output sudden change when protection is released: The present invention performs internal state reset and operating point synchronization at the moment of entering protection, including clearing the integral term, resetting the filter state, and updating the historical output buffer, so that the internal dynamic state and the actual output are coordinated, thereby reducing output sudden change, current impact and mechanical impact when protection is released.
[0038] (7) Problems and defects that still exist in the recovery process: The present invention continuously performs risk assessment during the protection and recovery phases. If the abnormal load conditions are met again, the protection parameters are reloaded and the operating point is synchronized again, avoiding the forced release of all driving capabilities after the fixed protection duration ends. This realizes a reentrant mechanism that adaptively extends or re-enters protection based on the actual contact state.
[0039] (8) In one embodiment, the present invention can independently perform compliance protection for multiple degrees of freedom. Different fingers and different joints can be configured with current evaluation threshold, drive output threshold, protection limit value and recovery time according to motor specifications, reduction ratio and load capacity, thereby improving the adaptability and flexibility of the whole hand compliance protection strategy. Attached Figure Description
[0040] Figure 1 A block diagram of a dexterous hand compliance protection system provided in an embodiment of the present invention; Figure 2 This is a flowchart of abnormal load identification and continuous confirmation provided in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the internal state reset and working point synchronization principle provided in an embodiment of the present invention. Figure 4 This is a diagram illustrating the transition between compliant protection and recovery states provided in an embodiment of the present invention. Figure 5 Joint position response curves under different test conditions under a step command provided in an embodiment of the present invention; Figure 6 The timing curves of the compliant protection triggering and gradual recovery process provided in the embodiments of the present invention are shown in the following: (a) is the change curve of the protection counter and the recovery counter, (b) is the curve of the actual drive output and the protection limit value after protection, and (c) is the change curve of the signed current. Figure 7The above is a comparison curve of compliance protection being turned on and off under interference conditions provided in the embodiments of the present invention, wherein (a) is the actual drive output comparison curve and (b) is the signed current comparison curve. Detailed Implementation
[0041] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely intended to explain the invention and not to limit it. Those skilled in the art will recognize that the invention can be practiced without some of these specific details. The following description of the embodiments is merely to provide a better understanding of the invention by illustrating examples of it.
[0042] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.
[0043] Example 1: This example provides a compliant protection method for a dexterous hand based on a drive current criterion. The method periodically executes steps S1 to S6.
[0044] Step S1: Obtain the pre-protection drive output And collect the current sampling value of the motor drive circuit. .
[0045] like Figure 2 As shown, first obtain the pre-protection drive output. and current characteristic quantities.
[0046] Step S2: Based on the protection pre-drive output The driving direction affects the current sampling value Direction normalization is performed to obtain the direction-normalized current evaluation quantity. .
[0047] Specifically, take the current sampling value Amplitude: ; According to the protection of the pre-drive output The sign of the current yields the direction-normalized current evaluation quantity: ; in, For a sign function, when When, take 1, when When -1 is taken; in one implementation, when When the value is 0, then... The positive and negative anomaly conditions are not met, therefore compliant protection is not triggered. This method is used to characterize the load state under the current driving direction. It is suitable for both single-ended current amplitude sampling and differential or signed current sampling.
[0048] In one implementation, the principle is as follows: Since the dexterous hand only has an encoder at the joint end and no encoder at the motor end, and there is a reduction mechanism and backlash between the motor and the joint, the joint end feedback cannot accurately reflect the transient motion state of the motor rotor; by normalizing the amplitude of the current sampling value according to the direction of the protection pre-drive output, so that... It can characterize the load state under the current driving direction, so that abnormal load can be judged without the need for motor speed or rotor position information.
[0049] Step S3: Determine the output before protection. and direction-normalized current evaluation quantity Check if the positive or negative abnormal conditions are met; if the positive abnormal condition is met, increment the positive continuous acknowledgment count and clear the negative continuous acknowledgment count; if the negative abnormal condition is met, increment the negative continuous acknowledgment count and clear the positive continuous acknowledgment count; if neither the positive nor negative abnormal conditions are met, clear both the positive and negative continuous acknowledgment counts; determine if the continuous acknowledgment count in the same direction has reached a preset number; if the continuous acknowledgment count in the same direction has reached a preset number, generate a compliant protection trigger signal; if the preset number has not been reached, continue to the next control cycle. like Figure 2 As shown, the positive outlier condition is: ; The reverse anomaly condition is: ; in, The current evaluation threshold is expressed in mA. To protect the front-drive output threshold (unit: mV), both forward and reverse abnormal conditions share the same threshold. and the same An abnormal load candidate state is only considered to exist when the protection drive demand is large and the corresponding current is consistently large.
[0050] Continuous acknowledgment counts are set separately for forward and reverse anomalies. When the forward anomaly condition is met, the forward continuous acknowledgment count is incremented by one, and the reverse continuous acknowledgment count is reset to zero; when the reverse anomaly condition is met, the reverse continuous acknowledgment count is incremented by one, and the forward continuous acknowledgment count is reset to zero; when neither the forward nor reverse anomaly condition is met, both the forward and reverse continuous acknowledgment counts are reset to zero. A compliant protection trigger signal is generated only when the continuous acknowledgment count in the same direction reaches a preset number.
[0051] In one implementation, continuity confirmation is performed by combining the directional normalized current evaluation value of the current control cycle and the directional normalized current evaluation value of the previous control cycle, including: obtaining the directional normalized current evaluation value of the previous control cycle. ; when the direction-normalized current evaluation quantity of the current control cycle and the previous control cycle All exceed the current evaluation threshold corresponding to the same direction. Furthermore, when the current protection drive output meets the drive output threshold condition for the corresponding direction, the continuous confirmation count for the corresponding direction is increased.
[0052] In one implementation, the principle is as follows: Traditional solutions rely solely on current thresholds for judgment, which are easily affected by starting shocks, commutation transients, current spikes, and sampling noise; by controlling the protection pre-drive output... As a criterion for control intent, the normalized current evaluation quantity is used. As a load response criterion, the two are used to make a joint judgment. Only when both the control intention and the load response meet the conditions will it be judged as an abnormal load candidate state, thereby avoiding the risk of misjudgment based on a single criterion.
[0053] Step S4: In response to the compliant protection trigger signal, the actual drive output after protection is... Limit to protection limit value Internally, it synchronizes the internal state with the actual drive output after protection. The corresponding protection working point.
[0054] like Figure 3 As shown, when abnormal load is continuously confirmed to reach the protection trigger condition, not only is the final drive output limited, but the internal state with memory is also processed synchronously.
[0055] In one implementation, the actual drive output after protection will be... Limit to protection limit value Includes: when hour, ; when hour, ; when hour, ; in, To protect the amplitude limit, the unit is mV.
[0056] The internal state includes at least one of the following: clearing or limiting the integral state, updating or synchronizing the internal state of the dynamic filter or notch filter to the state corresponding to the actual drive output after protection, and updating the current control output and historical output buffer. The protection operating point is the control output operating point corresponding to the actual drive output after protection, so as to synchronize the operating points.
[0057] In one implementation, the principle is as follows: Before protection is triggered, the position controller accumulates integral terms due to the continuous existence of position error; if only the final drive output is limited after protection is triggered while retaining the integral terms or filtering state before protection, a large instantaneous output is easily generated again after protection is released, causing current surges or backlash re-engagement shocks; this step synchronizes the internal state at the moment of entering protection, so that the internal dynamic state is consistent with the actual execution output, eliminating output jumps and mechanical shocks when protection is released.
[0058] Step S5: If a compliant protection trigger signal is not generated again after the protection duration ends, the actual drive output after protection is gradually increased according to the progressive recovery function. The allowed output range.
[0059] like Figure 4 As shown, after entering the protection state, the protection pre-drive output... The calculation is still performed periodically and used for subsequent abnormal load judgment; the actual protection output is driven after execution. It is then subject to protection limiting constraints. The protection state is maintained for a preset duration, and the actual drive output remains subject to protection limiting constraints until the end of this time.
[0060] After the protection period ends, if no further continuous abnormal triggering conditions are formed, a gradual recovery state is entered. This gradually restores the allowable range of the actual drive output from the protection limit to the normal drive range, rather than removing the limit all at once. Let the normalized recovery progress be... The smoothing factor changes from 0 to 1 as follows: ; The actual drive output is gradually increased after protection based on the smoothing factor. The upper and lower limits of the allowed output are set so that the upper and lower limits of the allowed output gradually recover from the protection limit range to the normal driving range. In other implementations, linear, exponential, sigmoid, or other continuous asymptotic functions may also be used.
[0061] In one implementation, the normalized recovery progress is... Incrementing from 0 to 1, increasing by 1 per control cycle. ,in This refers to the recovery time.
[0062] In one implementation, the allowable output upper limit of the normal drive range is set as follows: The lower limit of allowed output is The protection limit value is The maximum allowed output during the recovery phase. and allowed output lower limit satisfy: ; ; when hour, The allowed output range is the protection limit range; when hour, This allows the output range to be restored to the normal drive range.
[0063] Normalization recovery progress When the value reaches 1, the recovery is considered complete, and the system returns to normal control. The recovery progress is then normalized. Reset.
[0064] Step S6: During the execution of steps S4 and S5, continue to execute steps S1 to S3. If the abnormal condition in the same direction is met again and the preset number of times is reached, then re-enter step S4.
[0065] like Figure 4 As shown, abnormal load identification continues during both the protection and recovery phases. When the abnormal load conditions are met consecutively again, the protection persistence parameters and recovery parameters are reloaded, and internal state synchronization is performed again to normalize the recovery progress. The system is reset to zero and the protection duration is recalculated, re-entering a compliant protection state with limited drive capability. This forms a reentrant mechanism of "protection-recovery-reprotection".
[0066] For a multi-degree-of-freedom dexterous hand, each degree of freedom can independently set the current evaluation threshold, protection pre-drive output threshold, number of consecutive confirmations, protection limit value, protection duration and recovery time, and maintain its own risk count, protection status and recovery status.
[0067] In one engineering embodiment, the dexterous hand includes 6 degrees of freedom, with a control cycle of 1 ms. Current evaluation threshold. It can be set to 180mA to protect the front drive output threshold. It can be set to 1000mV, protection limit value. The value can be set to 200mV, the number of consecutive confirmations can be set to 10, the protection duration can be set to 200 control cycles, and the recovery time can be set to 300 control cycles. The mA and mV values mentioned above are exemplary scales, used only to illustrate one possible implementation method, and do not constitute a limitation on the scope of protection of this invention.
[0068] In one embodiment, the method of this embodiment is applied to a dexterous hand. The motor of the dexterous hand is a hollow cup DC brushed motor. No encoder is installed at the motor end, but an encoder is installed at the joint end. A reduction gear transmission mechanism with backlash is provided between the motor and the joint end. The method is based on the protection pre-drive output. and current sampling value Perform abnormal condition judgment.
[0069] Example 2: This example provides a compliant protection system for a dexterous hand based on a drive current criterion. For example... Figure 1 As shown, the system includes a control output calculation unit, a current sampling unit, a compliance protection unit, a drive output unit, a motor and a reduction transmission mechanism, and a joint position acquisition unit.
[0070] The control output calculation unit is connected to the compliant protection unit and is used to output the protection pre-drive output to the compliant protection unit. The current sampling unit is connected to the compliant protection unit and is used to collect the current sampling value of the motor drive circuit. And input compliance protection unit; compliance protection unit is connected to drive output unit, used to adjust according to... and Actual drive output after protection generation And output to the drive output unit; the drive output unit is connected to the motor and the reduction gear transmission mechanism, and is used to output according to The drive motor and reduction transmission mechanism; the joint position acquisition unit is connected to the control output calculation unit and is used to feed back joint position information to the control output calculation unit; the control output calculation unit is also used to receive target position commands.
[0071] The control output calculation unit is used to perform position control, feedforward, compensation, and filtering based on the target position command and joint position information, and outputs the protection drive output. .
[0072] The current sampling unit is installed on the motor drive circuit to collect the current sampling values of the motor drive circuit. .
[0073] The compliant protection unit includes a direction normalization module, a joint judgment module, a continuous confirmation module, a limiting protection module, a state synchronization module, and a gradual recovery module. The direction normalization module is used to determine the pre-protection drive output. The driving direction affects the current sampling value Direction normalization is performed to obtain the direction-normalized current evaluation quantity. The joint judgment module is used to determine the output before protection. and direction-normalized current evaluation quantity The system determines whether an abnormal condition is positive or negative. The continuous confirmation module increments the positive continuous confirmation count and resets the negative continuous confirmation count when a positive abnormal condition is met, and increments the negative continuous confirmation count and resets the positive continuous confirmation count when a negative abnormal condition is met. If neither positive nor negative abnormal condition is met, both the positive and negative continuous confirmation counts are cleared. It also checks if the continuous confirmation count in the same direction has reached a preset number; if so, it generates a compliant protection trigger signal. The limiting protection module responds to the compliant protection trigger signal by adjusting the actual drive output after protection. Limit to protection limit value The internal state synchronization module is used to synchronize the internal state of the control output calculation unit with the actual drive output after protection. The corresponding protection operating point; the progressive recovery module is used to gradually increase the actual drive output after protection if a compliant protection trigger signal is not generated again after the protection duration ends, according to the progressive recovery function. The allowed output range.
[0074] The drive output unit is used to output the actual drive after protection. Drive motor and reduction gear transmission mechanism. In one embodiment, the drive output unit includes a pulse width modulation and direction drive unit, used to output the drive according to the actual drive output after protection. Pulse width modulation and direction control are performed to drive the motor and the reduction gear transmission mechanism.
[0075] The motor and reduction gear mechanism are used to drive the joint mechanism to move under the control of the drive output unit. In one embodiment, the motor is a coreless DC brushed motor; no encoder is installed at the motor end; a reduction gear mechanism is installed between the motor and the joint position acquisition unit, and the reduction gear mechanism has backlash.
[0076] A joint position acquisition unit is disposed at the joint end and is used to acquire joint position information and feed it back to the control output calculation unit. In one embodiment, the joint position acquisition unit is a joint end encoder.
[0077] In addition, the continuous confirmation module continuously monitors the output before protection during the limiting and gradual recovery processes. and current sampling value An abnormal condition is determined. If the same abnormal condition is met again and the preset number of times is reached, a compliant protection trigger signal is generated, and the amplitude limiting protection module re-enters the protection state.
[0078] In one implementation, the control parameters and protection thresholds required by the compliant protection unit can be configured externally or pre-calibrated. Optionally, the system may also include a parameter configuration unit for configuring at least one of the control parameters and protection thresholds.
[0079] To verify the actual operating effect of the compliant protection method and system of the present invention, the compliant protection method of the present invention was compiled and implemented using the Keil development environment, and the compliant protection method program was obtained and downloaded to the Dexterous Hand embedded controller. Debugging data such as joint position, motor current, drive output, and protection and recovery counts were collected through the CAN bus. Comparative tests were carried out under approximately 50° step commands and different external interference conditions, and timing curve analysis was performed on the collected data.
[0080] During the approximately 50° step command response test, the initial joint position was approximately 11.78°, and the target joint position was approximately 61.71°. Three operating conditions were set up: compliant protection enabled with external interference applied; no external interference applied; and external interference applied but compliant protection disabled. Data such as joint position, protection counter, recovery counter, actual drive output after protection, and signed current were recorded. Figure 5 The joint position response curves under different test conditions under a step command are shown. Figure 6 The timing curves of the compliant protection triggering and gradual recovery process are shown, where (a) is the change curve of the protection counter and the recovery counter, (b) is the curve of the actual drive output and the protection limit value after protection, and (c) is the curve of the signed current change. Figure 7 The diagram shows a comparison curve of compliance protection being turned on and off under interference conditions, where (a) is the comparison curve of actual drive output and (b) is the comparison curve of signed current.
[0081] like Figure 5 The figure illustrates the joint position response under different test conditions under an approximately 50° step command. Under conditions without external disturbance, the joint responds normally to the command and reaches the target position. Under conditions with compliance protection enabled and external disturbance applied, the system triggers protection during movement, resulting in a phased change in the joint position response before continuing movement and reaching the target position. Under conditions with external disturbance applied but compliance protection disabled, the joint movement exhibits a noticeable pause before continuing towards the target position. This figure provides an overall view of the joint's motion response and positioning process under different test conditions.
[0082] like Figure 6As shown, during the test with compliant protection enabled, the system enters the protection processing phase approximately 258 ms after the step command is issued. After protection, the actual drive output is limited from approximately 1320 internal control units to approximately 200 internal control units and maintained for approximately 200 ms. After the protection duration phase ends, the system enters the gradual recovery phase, where the recovery counter gradually decreases, and the allowable range of the actual drive output gradually expands, completing recovery after approximately 300 ms. This test process demonstrates the limited drive limiting and gradual recovery mechanism of the method of this invention.
[0083] like Figure 7 As shown, comparing the drive output and signed current records when the compliant protection is on and off, under the condition of protection on, the actual drive output is limited, and the current recorded value drops from about 200 before triggering to about 30-50. Under the condition of protection off, the drive output in the corresponding recording range remains at a higher level. This result reflects the impact of protection limiting on the actual drive output and current response in this test.
Claims
1. A compliant protection method for a dexterous hand based on a driving current criterion, characterized in that, Perform the following steps periodically: Step S1: Obtain the pre-protection drive output And collect the current sampling value of the motor drive circuit. ; Step S2: According to The driving direction of Direction normalization is performed to obtain the direction-normalized current evaluation quantity. ; Step S3: Determine and Check if the positive or negative abnormal conditions are met; if the positive abnormal condition is met, increment the positive continuous acknowledgment count and clear the negative continuous acknowledgment count; if the negative abnormal condition is met, increment the negative continuous acknowledgment count and clear the positive continuous acknowledgment count; if neither the positive nor negative abnormal conditions are met, clear both the positive and negative continuous acknowledgment counts; determine if the continuous acknowledgment count in the same direction has reached a preset number; if the continuous acknowledgment count in the same direction has reached a preset number, generate a compliant protection trigger signal; if the preset number has not been reached, continue to the next control cycle. Step S4: In response to the compliant protection trigger signal, the actual drive output after protection is... Limit to the preset protection limit value Internally, and synchronize the internal state with... Corresponding protection working points; Step S5: If no compliant protection trigger signal is generated again after the preset protection duration ends, the protection will be gradually increased according to the progressive recovery function. The allowed output range; During the execution of steps S4 and S5, steps S1 to S3 are continuously executed. If a compliant protection trigger signal is generated again, the process re-enters step S4.
2. The method according to claim 1, characterized in that, In step S2, the direction normalization includes: taking the current sample value. amplitude ; and according to the protection pre-drive output The sign of the current is used to obtain the direction-normalized current evaluation quantity. ;in, For a sign function, when When, take 1; when When, take -1, when When the time is 0, the value is 0; in step S3, the abnormal conditions include positive abnormal conditions and negative abnormal conditions; the positive abnormal conditions are: The reverse anomaly condition is: ;in, The current evaluation threshold, To protect the front drive output threshold.
3. The method according to claim 2, characterized in that, In step S3, the continuous confirmation count includes: setting a positive continuous confirmation count and a reverse continuous confirmation count; when the positive abnormal condition is met, increasing the positive continuous confirmation count and clearing the reverse continuous confirmation count; when the reverse abnormal condition is met, increasing the reverse continuous confirmation count and clearing the positive continuous confirmation count; when neither the positive nor the reverse abnormal condition is met, clearing the positive and reverse continuous confirmation counts; performing continuous confirmation by combining the directional normalized current evaluation value of the current control cycle and the directional normalized current evaluation value of the previous control cycle; generating the compliant protection trigger signal only when the continuous confirmation count in the same direction reaches a preset number of times.
4. The method according to claim 1, characterized in that, In step S4, the actual drive output after protection is... Limit to protection limit value Includes: when hour, ; when hour, ; when hour, ; in, To protect the amplitude limit, the unit is mV.
5. The method according to claim 1, characterized in that, In step S4, the protection operating point is the control output operating point corresponding to the actual drive output after protection; the internal state synchronization includes at least one of the following: clearing or limiting the integral state, updating or synchronizing the internal state of the dynamic filter or notch filter to the state corresponding to the actual drive output after protection, and updating the current control output and historical output buffer.
6. The method according to claim 1, characterized in that, In step S5, the gradual recovery function includes: setting a normalized recovery progress. The smoothing factor changes from 0 to 1 as follows: The actual drive output after protection is gradually increased according to the smoothing factor. The allowable upper and lower limits of output are set so that the allowable upper and lower limits of output gradually recover from the protection limiting range to the normal driving range; the gradual recovery function is a linear function, exponential function, sigmoid function or other continuous gradual function; when the normalized recovery progress... When the progress reaches 1, the recovery is considered complete, and the system returns to normal control. The normalized recovery progress is then updated. Reset.
7. The method according to claim 6, characterized in that, If the abnormal conditions are met consecutively again during the recovery phase, the system re-enters a compliant protection state with limited drive capability, and the normalized recovery progress is restored. Reset to zero and restart the calculation of protection duration.
8. The method according to claim 1, characterized in that, The method further includes: independently setting at least one of the following for multiple degrees of freedom of the dexterous hand: current evaluation threshold, protection pre-drive output threshold, number of consecutive confirmations, protection limit value, protection duration, and recovery time; each degree of freedom maintains its own consecutive confirmation count, protection status, and recovery status.
9. The method according to claim 1, characterized in that, The method is applied to a dexterous hand, where the motor is a coreless DC brushed motor. The motor end of the dexterous hand does not have an encoder, but the joint end does. A reduction gear transmission mechanism with backlash is provided between the motor and the joint end. The method is based on the protection pre-drive output. and the current sampling value Perform abnormal condition judgment.
10. A dexterous hand compliant protection system based on drive current criterion, characterized in that, include: The control output calculation unit is used to output the protection pre-drive output. ; The current sampling unit is used to collect the current sampling values of the motor drive circuit. ; The compliant protection unit includes a direction normalization module, a joint judgment module, a continuous confirmation module, a amplitude limiting protection module, a status synchronization module, and a gradual recovery module; The direction normalization module is used to normalize the current sample values. Direction normalization is performed to obtain the direction-normalized current evaluation quantity. ; The joint judgment module is used to determine positive or negative abnormal conditions; The continuous confirmation module is used to perform forward continuous confirmation counting and reverse continuous confirmation counting, and to determine whether the continuous confirmation count in the same direction has reached the preset number. If it has, a compliant protection trigger signal is generated. The limiting protection module is used to respond to the compliant protection trigger signal and reduce the actual drive output after protection. Limit to protection limit value Inside; The state synchronization module is used to synchronize the internal state of the control output calculation unit with the actual drive output after protection. Corresponding protection working points; The progressive recovery module is used to gradually increase the actual drive output after protection if a compliant protection trigger signal is not generated again after the protection duration ends, according to the progressive recovery function. The allowed output range; The drive output unit is used to output the actual drive after the protection is applied. Drive motor and reduction gear transmission mechanism; The joint position acquisition unit is used to acquire joint position information and feed it back to the control output calculation unit.
Citation Information
Patent Citations
A DC brushed motor stall protection device and method based on current tracking
CN104242246B
