A method for preventing hand pinching in a robot servo system

Abstract

This application relates to the field of robot motion control and safety protection, and discloses a method for preventing hand pinching in a robot servo system. This method monitors the motor status in real time. When suspected interference is detected, a composite detection signal consisting of a base holding voltage, a DC preload component, and an AC detection component is injected into the drive motor. Through open-loop feedforward control, the feedback current is collected and the same-frequency component is extracted. The phase lag difference relative to the injected signal is calculated. Based on the magnitude of the phase lag difference, the system identifies whether the obstructing object is made of rigid or flexible material, and then executes a graded protection strategy of constant torque limiting or variable stiffness impedance control, respectively. This invention utilizes the principle of motional impedance to establish a mapping relationship between electrical phase and mechanical impedance, solving the problem of difficulty in distinguishing contact materials under low-speed conditions, achieving active compliance and safe escape, and improving the safety of human-machine interaction.

Description

Technical Field

[0001] This invention relates to the field of robot motion control and safety protection technology, specifically a method for preventing hand pinching in a robot servo system. Background Technology

[0002] With the increasing prevalence of service robots and collaborative robots in home, medical, and industrial settings, the safety of human-robot physical interaction has become a core indicator in system design. During robot grasping, handling, or joint movements, there is a risk of accidental clamping of human tissue (such as fingers or skin) at the end of the robotic arm or joint connections.

[0003] Currently, robot joint modules (servo motors) primarily employ passive collision detection technology based on current monitoring or torque observers. The most common approach is to set a fixed current threshold; when the motor bus current exceeds the set value, a collision is detected, triggering an emergency stop. However, this passive monitoring method has significant limitations when handling flexible clamping conditions. Human tissue has low structural stiffness and viscoelasticity; when clamping occurs, the load resistance increases gradually rather than abruptly, resulting in a gentle current rise. If the current threshold is lowered to improve sensitivity, false triggering can easily occur during normal start-stop acceleration or overcoming gear static friction; if the threshold is raised to ensure operational stability, the mechanical mechanism often causes crushing injury to the human body before the current reaches the alarm value.

[0004] Advanced solutions typically introduce dynamic models or disturbance observers to estimate external torque. These methods work well when the motor is running at high speeds, but under low-speed creep or static clamping conditions, the nonlinear frictional forces inside the reduction gearbox (including Coulomb friction and viscous friction) account for a significant portion of the total load torque, easily masking weak external contact forces and causing a significant decrease in the accuracy of the observer's estimation. Furthermore, both the current threshold method and conventional torque observers essentially detect the magnitude of the load torque, failing to identify the physical properties of the load. This means the control system cannot distinguish whether the external obstruction originates from a rigid object (such as a wall or table corner) or a flexible object (such as a human body). Due to the lack of ability to identify the stiffness of environmental contact, the system usually adopts a uniform hard braking strategy. In the event of flexible entanglement or clamping, a simple emergency stop and locking may cause secondary damage due to mechanical inertia or the inability to release stress.

[0005] While adding capacitive tactile skin or multi-dimensional torque sensors can solve the aforementioned problems, this significantly increases the size, weight, and manufacturing cost of the joint module, and also increases the complexity of signal transmission and wiring, making it difficult to widely apply in compact servo motors or consumer-grade robot products. Therefore, how to achieve contact material recognition at low speeds or zero speeds using existing motor drive circuits without adding additional sensor hardware, and to implement differentiated safety protection accordingly, is a pressing technical problem to be solved in the field of robot flexible control. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a method for preventing hand pinching in robot servo systems, solving the problem that existing technologies struggle to distinguish between rigid collisions and flexible clamping at low speeds or when stationary.

[0007] The first aspect of this invention provides a method for preventing hand pinching in a robot servo system. The method includes: real-time monitoring of the operating status of the drive motor; if the system is determined to enter a suspected interference state, suspending the current trajectory control and acquiring a base holding voltage for balancing the gravity load; injecting a composite detection signal into the drive motor, the composite detection signal being constructed by superimposing a DC preload component and an AC detection component based on the base holding voltage; acquiring the feedback current of the drive motor, extracting a current response component with the same frequency as the AC detection component, and calculating the phase lag difference of the current response component relative to the AC part of the composite detection signal; comparing the phase lag difference with a preset material property threshold, identifying the material property of the obstructing object, and executing a corresponding graded protection strategy.

[0008] In the monitoring phase, the system calculates a theoretical reference current based on a rigid body dynamics model, which includes inertial torque, viscous damping torque, and frictional torque terms. By calculating the dynamic current deviation between the actual bus current and the theoretical reference current, and combining this with the ratio of the control command change rate to the position feedback change rate (i.e., the blocking gradient coefficient), the system determines whether it has entered a suspected interference state. This method utilizes a dual verification of the model and the gradient to eliminate load fluctuation interference caused by normal acceleration and deceleration of the system.

[0009] Regarding the control logic for signal injection, this invention employs an open-loop feedforward mode. The direction of the DC preload component is consistent with the direction of the angular velocity immediately before triggering the detection, and its amplitude is set to the critical voltage value for eliminating the backlash in the transmission gear set. The AC detection component uses a sine wave lower than the motor's electrical cutoff frequency. During injection, the controller locks the current loop integral term and cuts off the speed loop output, directly using the composite signal as the torque shaft voltage command to prevent the feedback regulation of the closed-loop controller from suppressing the high-frequency detection signal. Furthermore, the system sets a signal injection time window; after the timeout, injection is forcibly stopped to protect the motor coils.

[0010] In signal analysis, the feedback current, after bandpass filtering and windowing, is processed using a discrete transform algorithm. By constructing in-phase and in-frequency sine and cosine reference sequences within the controller, the projection of the current sequence is calculated to obtain the in-phase and quadrature components. The absolute phase angle is calculated using a four-quadrant arctangent operation, and the pre-calibrated system-specific phase shift calibration constant is subtracted to obtain the phase hysteresis difference.

[0011] This invention utilizes phase lag difference to characterize the mechanical impedance characteristics of contact conditions: under rigid contact conditions, the motor rotor cannot vibrate, the back electromotive force approaches zero, and the phase lag difference is small; under flexible contact conditions, the flexible load allows the rotor to vibrate at the same frequency, and the back electromotive force induced in the winding changes the circuit impedance characteristics, resulting in an increase in the phase lag difference.

[0012] Based on the above identification results, the system performs graded protection: when it is determined to be a rigid obstruction, it switches to constant torque holding mode and limits the output torque; when it is determined to be a flexible clamping, it performs variable stiffness impedance control, attenuates the position loop proportional gain and the velocity loop differential gain, and generates a reverse escape trajectory to drive the motor away from the obstructing object.

[0013] A second aspect of the present invention provides an anti-pinch device for a robot servo system, comprising a status monitoring module, a signal injection module, an impedance analysis module, and a safety execution module.

[0014] The status monitoring module is used to collect motor status data, calculate dynamic current deviation and blocking gradient coefficient to determine suspected interference state; the signal injection module is used to construct a composite detection signal in the suspected interference state and inject it into the motor drive circuit in an open-loop feedforward manner; the impedance analysis module is used to extract specific frequency components in the feedback current and calculate phase lag difference; the safety execution module is used to distinguish between rigid contact and flexible contact based on phase lag difference, and call torque limiting strategy or variable stiffness impedance control strategy respectively.

[0015] This invention provides a method for preventing hand pinching in a robot servo system. It has the following beneficial effects:

[0016] 1. This invention solves the technical problem that traditional current threshold detection methods cannot distinguish the material of obstructions. By actively injecting a high-frequency AC detection signal under suspected interference conditions and analyzing the phase lag difference between the feedback current and the injected signal, the physical mechanism of the back electromotive force generated by a flexible load changing the circuit impedance characteristics is utilized to achieve effective identification of rigid and flexible contacts. This method is independent of motor speed and can accurately determine the properties of obstructions even at zero or low speeds, avoiding accidental shutdowns caused by setting the single torque threshold too low or safety hazards caused by setting it too high.

[0017] 2. This invention improves the signal-to-noise ratio and execution accuracy of signal detection. At the signal construction level, by superimposing a DC preload component sufficient to eliminate gear backlash, the nonlinear interference of the mechanical dead zone of the transmission system on the high-frequency response is eliminated. At the control level, an open-loop feedforward method is used to inject the signal and lock the current loop integral term, preventing the closed-loop controller from suppressing the high-frequency detection signal. These measures together ensure that the weak motional impedance characteristics can be accurately extracted by the controller, guaranteeing the accuracy of phase calculation. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the main framework of the present invention;

[0019] Figure 2 This is a detailed flowchart of the present invention for monitoring the operating status of a drive motor and determining suspected interference states;

[0020] Figure 3 This is a flowchart of the signal injection module of the present invention;

[0021] Figure 4 This is a signal processing logic block diagram for the impedance analysis module of the present invention, which performs frequency domain response extraction and phase feature decoupling.

[0022] Figure 5 This is a time-domain response waveform diagram of the signal injection process of the present invention;

[0023] Figure 6 This is a diagram showing the impedance phase characteristic distribution of the present invention. Detailed Implementation

[0024] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0025] See attached document Figure 1 , Figure 1 This is a schematic diagram of the hardware architecture of a robot servo motor anti-pinch control system according to an embodiment of the present invention. The main hardware components of the system include a drive motor, a reduction gear set, a position sensor, a current sampling circuit, and a main control unit.

[0026] The drive motor serves as the system's power source, powered by an H-bridge drive circuit. The input of the reduction gear set is mechanically coupled to the drive motor's rotor shaft, while the output is connected to the robot's joints or end effector. The drive motor's output torque is amplified by the reduction gear set to drive the movement of the external load.

[0027] Position sensors are mounted on the output shaft of the reduction gear set or the rotor shaft of the drive motor to collect joint angle position information in real time. The position sensors can be potentiometers, magnetic encoders, or photoelectric encoders. A current sampling circuit is connected in series in the drive motor bus circuit or phase circuit, and is equipped with sampling resistors and signal conditioning circuits to collect the current flowing through the drive motor in real time.

[0028] The main control unit is electrically connected to the drive motor circuit, position sensor, and current sampling circuit. The main control unit integrates a microprocessor, which includes a PWM generator, analog-to-digital converter, and processing core.

[0029] The main control unit internally operates control logic, which is functionally divided into multiple processing modules. For example... Figure 1 As shown, the module includes a status monitoring module, a gradient analysis module, a signal injection module, an impedance analysis module, and a safety execution module.

[0030] The status monitoring module is configured to acquire real-time motion control commands and motor feedback status, and calculate the theoretical reference current based on a preset dynamic model. The gradient analysis module is configured to monitor the rate of change of control commands and the rate of change of position feedback, and calculate their ratio to generate a blocking gradient coefficient.

[0031] The signal injection module is configured to apply a composite voltage signal containing a DC preload component and an AC probe component to the drive motor when an abnormal obstruction is detected. The impedance analysis module is configured to acquire the motor circuit current feedback signal and extract the phase response characteristics at a specific frequency. The safety execution module is configured to determine the obstacle attributes based on the phase response characteristics and generate corresponding protection action commands accordingly.

[0032] See attached document Figure 2 , Figure 2 This is a flowchart of a robot servo motor anti-pinch control method according to an embodiment of the present invention. It includes the following steps:

[0033] S10, the status monitoring module collects the target motion command, actual current and actual position of the drive motor in real time, and calculates the theoretical reference current based on the motor dynamics model;

[0034] S20, the gradient analysis module calculates the ratio of the control command change rate to the position feedback change rate in real time to obtain the blocking gradient coefficient, and compares the blocking gradient coefficient with the preset gradient threshold.

[0035] S30, when the blocking gradient coefficient is greater than the gradient threshold, the system is determined to have entered a suspected interference critical state. The signal injection module suspends the conventional control integral term and injects a composite detection signal containing DC preload and AC detection components into the drive motor.

[0036] S40, the impedance analysis module synchronously acquires the feedback current signal of the drive motor, extracts the current response component with the same frequency as the AC detection component, and calculates the phase lag difference of the current response component relative to the AC part of the composite detection signal.

[0037] S50, the safety execution module compares the phase hysteresis difference with the preset rigid threshold and flexible threshold, identifies the material properties of the obstructing object based on the comparison result, and executes the corresponding graded protection strategy.

[0038] See attached document Figure 2 In step S10, the state monitoring module performs dynamic benchmark construction and real-time state monitoring. The core logic of this step is to construct a feedforward dynamic model based on the physical characteristics of the motor, and use the analytical solution of the physical equation to predict the current performance of the motor under unobstructed conditions, thereby decoupling and separating the reasonable current fluctuations caused by the mechanical structure itself (such as acceleration and deceleration inertia, gear oil viscosity) from the abnormal current fluctuations caused by external collisions.

[0039] This process specifically includes the following sub-steps:

[0040] In sub-step S101, the system synchronously acquires motion command data and feedback status data of the drive motor within each control cycle. The motion command data originates from the upper-level trajectory planner, including the target angular velocity at the current moment. and target angular acceleration The feedback status data comes from sensor sampling, including the current actual position measured by the position sensor. And the actual bus current measured by the current sampling circuit. To eliminate the interference of signal noise on the differential operation, for the target angular acceleration... If the upper layer does not directly provide the speed command, the status monitoring module uses a second-order difference method combined with a low-pass filter to process the continuous speed commands. The filter cutoff frequency is set to 1 / 5 to 1 / 10 of the control frequency to balance the real-time performance and smoothness of the signal.

[0041] In sub-step S102, a current observation model based on rigid body dynamics is established. The input to this model is the motion command data collected above ( The output is the theoretical reference current. Based on the principle of electromechanical energy conversion, this model decomposes the electromagnetic torque of the motor into three parts: the acceleration torque to overcome inertial load, the velocity-dependent torque to overcome viscous damping, and the constant torque to overcome inherent friction.

[0042] Theoretical reference current The calculations are performed based on the following discrete dynamic equations:

[0043] ;

[0044] : The theoretical reference current calculated in the k-th control cycle, in amperes (A). This value represents the net current required to maintain the current motion state without external force contact.

[0045] Torque constant of the drive motor, measured in Newton-meters per ampere (N·m / A). This parameter is an inherent property of the motor and is usually determined directly by the technical specifications provided by the motor manufacturer, or obtained by measuring the linear relationship between current and output torque during a stall test.

[0046] The equivalent moment of inertia of the motor rotor and reduction gear set referred to the motor shaft, expressed in kilograms per square meter (kg·m). 2 This item reflects the system's ability to resist speed changes, specifically the magnitude of the change in the dominant current under rapid acceleration and deceleration conditions.

[0047] : Target angular acceleration in the kth control cycle, in radians per square second (rad / s) 2 ).

[0048] The equivalent viscous damping coefficient of the motor and transmission system, measured in Newton-meter-seconds per radian (N·m·s / rad). This parameter reflects the resistance of gearbox grease viscosity and bearing damping to motor operation, and the resulting current consumption is proportional to the rotational speed.

[0049] : Target angular velocity in the kth control cycle, in radians per second (rad / s).

[0050] Coulomb friction torque, inherent within the transmission system, is measured in Newton-meters (N·m). This represents the basic static friction resistance during system startup or low-speed operation.

[0051] sgn(·): The sign function, used to determine the direction of the frictional torque, ensuring it is always opposite to the direction of motion. In engineering implementation, to avoid jitter near zero velocity, this sign function can be smoothly replaced by a hyperbolic tangent function or a dead-zone function.

[0052] In sub-step S103, the dynamic parameters are... , and Calibration is performed. In practice, an offline system identification method is used: the robot joints are subjected to a set of excitation movements containing multi-frequency sinusoidal sweeps (Chirp signals) or trapezoidal velocity curves while in an unloaded (no external contact) state. The current throughout the entire process is recorded synchronously. ,speed and acceleration The data sequence is used to construct an overdetermined system of equations. The least squares method (LSM) is then used to solve this system of equations, thereby identifying the parameter combination that minimizes the root mean square error of the model prediction. Once calibrated, these parameters will be stored as constants in the controller's non-volatile memory until the system undergoes mechanical maintenance or parts replacement and are recalibrated.

[0053] In sub-step S104, the actual bus current is calculated. Compared with theoretical reference current Dynamic current deviation between This deviation value In physical essence, it is equivalent to the observation residual of a Disturbance Observer (DOB). When the system is in a normal, contactless operating state, since the model parameters have been calibrated, Theoretically, it approaches zero or fluctuates within a very small noise range; when external contact occurs, the actual current... This will include additional load components, resulting in This dynamic current deviation will be significantly increased. It will be cached in the system register as data to support subsequent steps in determining whether the system is in a nonlinear load region.

[0054] See attached document Figure 2 In step S20, the gradient analysis module performs an initial screening of abnormal states based on the command-response gradient. This step utilizes the transient derivative relationship between the input command and the output response in the control loop to construct an observation index reflecting the dynamic stiffness of the system, identifying the tendency of the mechanical transmission chain to be hindered before the current reaches the thermal protection threshold. This process specifically includes the following sub-steps:

[0055] In sub-step S201, the gradient analysis module extracts key state variables from the current control cycle and the previous control cycle in real time. The system obtains the output control variable (PWM duty cycle) of the PID controller and the feedback angle of the position sensor, respectively. Considering the sensitivity of direct differentiation to high-frequency noise, the system first performs a process of length [missing value] on the original data sequence before calculating the rate of change. (For example Moving average filtering (up to step 5) is applied to suppress encoder quantization noise and current loop switching noise. Subsequently, the system uses a first-order backward difference method to calculate the rate of change of the control command. With position feedback rate of change Rate of change of control commands and position feedback rate of change The calculation is based on the following formula:

[0056] ;

[0057] ;

[0058] The physical meanings of the symbols in the formula are defined as follows:

[0059] Current number The voltage duty cycle command output by the PID controller to the H-bridge drive circuit at any time is normalized to the range of [-100, 100].

[0060] Current number The actual rotation angle of the motor at any given moment, in radians;

[0061] LPF(·) This represents a low-pass filter operator used to extract the low-frequency trend term of a signal and filter out noise components with frequencies higher than half the control frequency.

[0062] In sub-step S202, the blocking gradient coefficient is constructed based on the aforementioned rate of change. From a control theory perspective, under normal operating conditions, the system's input and output increments follow its open-loop gain characteristics, remaining within the linear proportional range. When mechanical blocking occurs, the motor rotor is physically locked (…). ), while the position closed-loop controller, due to position tracking error, has a proportional term ( The integral term (l) and the integral term (l) will cause the output voltage to... It rises sharply. At this point, the rate of change of the control quantity... With the rate of change of position There are various serious mismatches.

[0063] Blocking gradient coefficients The calculation is based on the following formula:

[0064] ;

[0065] The physical meaning and value definitions of each symbol in the formula are as follows:

[0066] No. The blocking gradient coefficient calculated at each moment is a physical quantity that characterizes the increase in control energy required per unit displacement increment and essentially reflects the transient equivalent stiffness at the load end.

[0067] Numerical stability regularization parameter, with a value of 10×10 -6This parameter is used to handle the division-by-zero singularity problem when the motor is completely stationary or at the zero-crossing point of commutation, ensuring the convergence of numerical calculations.

[0068] In sub-step S203, the blocking gradient coefficients calculated in real time are... With the preset gradient threshold Compare. Gradient threshold The calibration is determined using the maximum load step response method: with the motor connected to the maximum allowable rated inertia load and without external interference, a step position command is input, and the maximum gradient coefficient value generated at the moment of startup is recorded. Set gradient threshold ,in For safety, the value ranges from [1.2, 1.5]. This setting ensures that the system can tolerate normal rapid acceleration conditions and is triggered only when there is a fundamental change in load characteristics.

[0069] In sub-step S204, when the comparison result meets the condition... When the system is deemed to have entered a suspected interference critical state, the main control unit performs an integral separation operation, which involves adjusting the integral gain coefficient in the PID controller. The value of the integral accumulator register is temporarily reset to zero or frozen. This operation aims to prevent the integral error from accumulating indefinitely due to continuous obstruction of the motor during subsequent active detection, thereby avoiding excessive rebound torque (integral saturation) at the moment of unblocking. Simultaneously, the system state machine transitions to active signal injection mode, initiating a secondary verification of the obstacle's physical properties.

[0070] See attached document Figure 2 In step S30, the signal injection module actively injects the composite detection signal. This step is the core control link of the invention, utilizing the superposition principle to maintain the macroscopic motion trend while constructing a controlled "force-electric" dynamic testing environment at the microscopic level. Its core purpose is to solve the problem that the mechanical backlash of the reduction gearbox causes weak high-frequency detection signals to be absorbed by the transmission gap and unable to be transmitted to the load end. This process specifically includes the following sub-steps:

[0071] In sub-step S301, at the instant the system is determined to have entered a suspected interference critical state, the signal injection module takes over control of the H-bridge drive circuit. To prevent sudden torque changes during control mode switching, the system executes a smooth switching logic: first, it locks the current proportional and derivative outputs of the PID controller and temporarily suspends the integral term (i.e., stops accumulation and updates). The system reads the total output value of the PID controller at the instant of switching and defines it as the base holding voltage. .Should Its function is to provide a reference torque that balances the current gravity and inertial load, ensuring that the robotic arm will not fall or bounce unexpectedly when the control signal returns to zero during subsequent exploration.

[0072] In sub-step S302, the signal injection module constructs a composite probe voltage signal by superimposing a DC preload component and an AC probe component on the base holding voltage. In digital microprocessors, this signal is generated using direct digital frequency synthesis (DDS) or a sine lookup table (LUT). Its mathematical expression follows the formula:

[0073] ;

[0074] The physical meaning and value definitions of each symbol in the formula are as follows:

[0075] : The final composite voltage command applied to both ends of the motor at all times;

[0076] : The output voltage hold value of the PID controller at the trigger detection moment;

[0077] DC preload voltage amplitude, which represents the critical voltage threshold required to overcome static friction and gear backlash in the transmission system;

[0078] sgn The sign function takes the sign direction of the base voltage to ensure that the direction of the applied preload torque is consistent with the original motion trend and the direction of the gravity load it is subjected to, thereby preventing gears from knocking in the opposite direction.

[0079] The amplitude of the AC detection signal, expressed in volts or PWM duty cycle;

[0080] : The frequency of the AC detection signal, measured in Hertz (Hz);

[0081] The time variable in the detection process is used to control the period. The step size increases incrementally.

[0082] In sub-step S303, for the DC preload component The specific implementation method is determined as follows. Due to the inherent minute mechanical backlash at the gear meshing points inside the precision reducer, at zero speed or near commutation, minute AC vibration signals are absorbed by this backlash, preventing them from being transmitted to the external load and creating a nonlinear dead zone in signal transmission. To obtain accurate... During the initialization phase, the system executes a dead-zone calibration procedure: with the motor unloaded, the voltage command is gradually increased in very small steps (e.g., 0.1% of the rated voltage) until the position sensor detects the first micro-motion (e.g., the encoder value changes by more than one count bit). The voltage value at this point is recorded and multiplied by a reliability coefficient (e.g., 1.1 to 1.2) as the fixed value. Parameters. By superimposing this voltage, the system forces all gear pairs inside the reducer to maintain a tight meshing state on one side, establishing a near-rigid torque transmission channel.

[0083] In sub-step S304, the AC detection component is targeted. The specific implementation will be carried out by selecting the parameters.

[0084] frequency The selection criteria follow the following constraints:

[0085] First, it must be higher than the system's first-order mechanical natural frequency (usually <15Hz) to avoid triggering structural resonance;

[0086] Second, it must be lower than the current loop sampling frequency. This frequency is 1 / 10 (an engineering margin following the Nyquist sampling theorem) and much lower than the cutoff frequency determined by the motor's electrical time constant, to ensure that the current loop can effectively respond to voltage excitation. In practice, this frequency is set to a fixed value between 40Hz and 80Hz.

[0087] Amplitude The selection principle is to minimize the impact on mechanical motion while ensuring that the current sensor has a sufficient signal-to-noise ratio (SNR). Setting This results in a theoretical current ripple amplitude greater than the quantization noise floor of the current sensor (e.g., greater than 3 times the minimum resolution), while the corresponding torque ripple is less than the system's rated torque. By injecting this high-frequency disturbance into the pre-tensioned drive train, the system is able to linearly map the dynamic impedance characteristics of the mechanical load into the current response of the electrical circuit.

[0088] See attached document Figure 2 In step S40, the impedance analysis module performs frequency domain impedance response extraction and phase analysis. This step is a key processing step in determining the physical properties of obstacles in this invention. Its core technical principle lies in utilizing the mapping relationship between mechanical impedance and electrical impedance. By analyzing the dynamic response of the motor circuit current to a specific frequency voltage signal, the contact stiffness and damping characteristics, which are difficult to measure directly, are transformed into phase characteristic values ​​that can be accurately calculated. This process specifically includes the following sub-steps:

[0089] In sub-step S401, the impedance analysis module synchronously acquires the drive motor circuit current data during the composite signal injection. To ensure the accuracy of the frequency domain analysis, the system activates a current field of length [length missing] simultaneously with the injection of the AC probe component. A circular data buffer, at a frequency significantly higher than the detection frequency. sampling rate Record real-time current sequences (e.g., 10 kHz). During the data acquisition process, the system performs DC removal preprocessing on the raw current data: a moving average filter is used to calculate the DC component within the current window. and using the formula Eliminate voltage maintained by base and DC preload voltage The generated static current retains only the dynamic response current excited by the AC probe voltage. .

[0090] In sub-step S402, a single-point discrete Fourier transform algorithm is used to analyze the dynamic response current containing broadband noise. Extracting the injection frequency Strictly corresponding fundamental frequency components. Compared to full-spectrum FFT transform, this method significantly reduces computational resource consumption, making it suitable for real-time operation of embedded microcontrollers. The system constructs sinusoidal and cosine reference sequences with the same frequency as the injected signal, respectively, and performs cross-correlation integration with the acquired current sequence. To ensure orthogonality and eliminate spectral leakage, the integration window length is... The selection of [a specific parameter] must satisfy the integer period sampling condition, i.e. It is a positive integer (e.g., take...) Up to 5 signal cycles). In-phase component of the current response. Orthogonal components The calculation is based on the following formula:

[0091] ;

[0092] ;

[0093] The physical meaning and value definitions of each symbol in the formula are as follows:

[0094] : The in-phase projection component of the current response vector in the reference coordinate system;

[0095] : The orthogonal projection components of the current response vector in the reference coordinate system;

[0096] The total number of sampling points within the integration window is determined by the formula. round Sure;

[0097] No. The DC-free current value at each sampling point;

[0098] The injected AC detection frequency;

[0099] .

[0100] In sub-step S403, the effective phase hysteresis difference between the current response vector and the voltage injection vector is calculated based on the extracted components. Considering the inherent signal propagation delay in the hardware circuitry (sampling resistors, operational amplifier circuits, ADC sample-and-hold), this delay introduces a non-load-dependent system phase shift. During factory calibration, the inherent phase shift of the system at that frequency is pre-acquired through an unloaded injection test. And store it. The final phase lag difference calculation needs to deduct this system error.

[0101] Phase lag difference The calculation is based on the following formula:

[0102] ;

[0103] The physical meaning and value definitions of each symbol in the formula are as follows:

[0104] The final calculated net phase lag angle caused by the external load, in degrees or radians;

[0105] The arctangent function in the four quadrants has a range of values ​​of 100.

[0106] The reference phase of the injected voltage signal is usually set to 0 in the code generation logic;

[0107] The inherent phase shift calibration constant of the system is derived from offline calibration data.

[0108] In sub-step S404, the calculated phase lag difference is... The physical meaning is mapped and verified. From the perspective of electromechanical coupling principles, when the motor load is a purely rigid obstruction (such as a metal limiter), the motor rotor is physically locked, the back electromotive force component is zero, and the motor's equivalent circuit degenerates into a pure resistive-inductive (RL) series model. In this case, the current phase lag is determined only by the winding inductance, exhibiting a small, stable value (rigid reference phase shift). When the motor load is a flexible biological tissue (such as a finger), the obstruction exhibits viscoelasticity, and the external mechanical damping... and stiffness Through electromechanical conversion coefficient Mapped onto the electrical side, this causes a phase shift in the dynamic back electromotive force, resulting in a significant increase in the total circuit impedance angle. This physical phenomenon leads to a phase lag difference. This becomes a sensitive indicator for distinguishing between hard and soft objects. To improve anti-interference capabilities, the system performs continuous calculations on five... The value is then subjected to median filtering to output the final phase decision value.

[0109] See attached document Figure 2 In step S50, the safety execution module executes a graded safety execution strategy based on material identification. This step is the decision-making terminal of the control flow of this invention, and its core logic is to convert the phase hysteresis... As a decision variable, the underlying control law of the servo system is dynamically switched within milliseconds by comparing it with preset physical attribute boundaries. This process specifically includes the following sub-steps:

[0110] In sub-step S501, the safety execution module reads the phase hysteresis difference output by the impedance analysis module. This is then mapped to a preset material property range. Two key decision thresholds are preset in system memory: a rigidity threshold and a... and flexible judgment threshold To ensure the accuracy of the determination, these two thresholds are determined at the factory stage using a "two-point calibration method":

[0111] Rigid calibration: Control the motor to push the metal limit block at a low speed (e.g., 5 RPM) until it stalls, and record the average phase lag value after stabilization. and standard deviation ,set up .

[0112] Flexible calibration: Control the motor to compress a standard silicone block with a Shore hardness of 10 to 30 (simulating human muscle hardness) at the same speed, and record the average phase hysteresis. and standard deviation ,set up .

[0113] The system determines the status based on the following logic:

[0114] like The current working condition is determined to be "rigid resistance";

[0115] like The current working condition is determined to be "flexible clamping";

[0116] like It was determined to be a "fuzzy transition zone".

[0117] At this point, to prevent misjudgment caused by signal noise, the system starts a counter. Only when there are N consecutive control cycles (e.g. Only when the phase values ​​of all elements fall within this region or higher will the element be classified as "flexible clamping" to execute a conservative strategy; otherwise, the current state will be maintained.

[0118] In sub-step S502, when the determination result is a rigid blockage, it indicates that the robot end effector has encountered a hard obstacle in the environment or its own mechanical limit. The main risk at this time is overcurrent causing the motor windings to burn out or the gearbox teeth to crush. The safety execution module immediately terminates the signal injection and switches to constant torque holding mode.

[0119] In this mode, the controller's objective shifts from position tracking to current loop limiting control. The strategy is as follows: Maintain the current position command unchanged, but clamp the saturation amplitude of the PID controller output, limiting the motor output torque to 30% to 50% of the rated torque. This strategy utilizes the remaining torque to maintain the current robotic arm posture against gravity while preventing continuous current overload until a reset command is received from the upper-level controller or the current naturally decreases (the obstruction disappears).

[0120] In sub-step S503, when the determination result is "flexible gripping," indicating that the robot is squeezing a human body or other flexible object, the system immediately triggers the highest-priority active compliant withdrawal strategy. This strategy includes two actions executed in parallel:

[0121] First, implement variable stiffness impedance control. The system immediately terminates the injection of all probe signals and adjusts the position loop proportional gain. and velocity loop differential gain Dynamic decay to nominal value Up to 20%. According to control theory, position gain Physically equivalent to the stiffness coefficient of a virtual spring, differential gain This is equivalent to a virtual damping coefficient. By significantly reducing these two parameters, the robotic arm instantly changes from a rigidly connected to a softly floating state in terms of electrical characteristics, allowing the gripped object to passively release the squeezing pressure by pushing the robotic arm with a small reaction force.

[0122] Second, a reverse escape trajectory is generated, driving the motor to actively move away from the obstacle. (Reverse target position) The calculation is based on the following formula

[0123] ;

[0124] The physical meaning and value definitions of each symbol in the formula are as follows:

[0125] Instructions for the target location of safe withdrawal;

[0126] The actual joint position at the moment the protection is triggered;

[0127] sgn The movement direction symbol at the moment before the fault occurs (i.e. before entering the suspected state). This direction is recorded in the historical state register to ensure that the retraction direction is strictly opposite to the compression direction, so as to avoid false actions caused by speed noise at the moment of the fault.

[0128] : The preset safe retraction distance, which is set to the maximum angular velocity of the joint. 1.5 times the product of the average human neural reaction time (approximately 0.2 seconds) usually corresponds to to The mechanical rotation angle.

[0129] In sub-step S504, after executing the aforementioned graded protection actions, the safety execution module reports a fault status code to the robot's main controller and locks the current control state machine. At this time, the underlying servo drivers will block all external commands except for fault reset, preventing the upper-level planning algorithm from continuing to send erroneous motion commands due to the failure to detect a fault. Only when the system receives a clear manual confirmation signal will the system reinitialize the dynamic model parameters and return to the normal position control mode.

[0130] See attached document Figure 2 The condition monitoring module performs dynamic benchmark construction and real-time condition monitoring. This step utilizes the feedforward control principle to construct a dynamic model based on the physical characteristics of the motor. This model serves as a virtual reference object, running in parallel within the actual motor control loop. Its basic principle is as follows:

[0131] Using analytical rigid body dynamics equations, the theoretical torque required for the motor to overcome its own inertia, viscous damping, and friction under ideal operating conditions without external obstruction is calculated in real time and converted into a corresponding current value. By comparing the actual current with the theoretical current, the system can isolate reasonable fluctuations caused by the mechanical structure's own motion characteristics (such as inertial current generated by rapid acceleration), thereby accurately extracting the disturbance component generated solely by external contact. This process specifically includes the following sub-steps:

[0132] In sub-step S101, the status monitoring module synchronously collects motion command data and feedback status data of the drive motor in each control cycle. The motion command data comes from the upper-level trajectory planner, including the target angular velocity at the current moment. and target angular acceleration The feedback status data comes from sensor sampling, including the current actual position measured by the position sensor. And the actual bus current measured by the current sampling circuit. Considering the amplification effect of numerical differentiation on noise, if the upper-level controller does not directly issue acceleration commands, the state monitoring module uses a combined algorithm of "second-order difference + low-pass filtering" to obtain the acceleration data. The low-pass filter employs a second-order Butterworth structure, with the cutoff frequency set to 1 / 10 of the servo control frequency (e.g., 100Hz) to suppress high-frequency quantization noise while preserving true dynamic characteristics.

[0133] In sub-step S102, a current observation model based on rigid body dynamics is established. This model is a linear parameter variation (LPV) model, and its inputs are motion state variables. The output is the theoretical reference current. The model is based on the torque balance equation, which decomposes the total electromagnetic torque of the motor into inertial terms, viscous damping terms, and Coulomb friction terms.

[0134] Theoretical reference current The calculation is based on the following formula:

[0135] ;

[0136] The physical meaning and value definitions of each symbol in the formula are as follows:

[0137] : No. The theoretical reference current calculated for each control cycle is in amperes (A).

[0138] The torque constant of the drive motor, in units of... This parameter is determined by the motor's magnetic circuit design and can be obtained by consulting the motor's manufacturer's specifications.

[0139] The equivalent moment of inertia of the motor rotor and reduction gear set referred to the motor shaft, in units of 1.

[0140] The equivalent viscous damping coefficient of the motor and transmission system, in units of... ;

[0141] The amplitude of the inherent Coulomb frictional torque within the transmission system, in units of... ;

[0142] The hyperbolic tangent function is used to approximate discontinuous sign functions. .in This is the slope smoothing factor (e.g., a value between 5.0 and 10.0). This function is introduced to address the numerical jitter problem caused by the sgn function during zero-velocity crossing, ensuring that the calculated current command is smooth and continuous.

[0143] In sub-step S103, the dynamic model parameters are... , and Perform identification (training). This process is equivalent to the offline training phase of the model, and the specific implementation steps are as follows:

[0144] Data Acquisition: Controlling the robot joints to execute a segment of sinusoidal chirp excitation motion or trapezoidal acceleration / deceleration motion containing multiple frequency components under no-load conditions and without external contact. Recording time-series data of the entire process. .

[0145] Constructing the regression equation: Rewriting the dynamic equation into a linear regression form .in:

[0146] Observation vector ;

[0147] Vector of parameters to be estimated ;

[0148] Regression Matrix Each row is constructed .

[0149] Parameter determination: The optimal parameter estimates are obtained using the least squares method.

[0150] ;

[0151] Parameter solidification: the calculated parameters Solving for physical parameters And it is stored in the controller's EEPROM.

[0152] In sub-step S104, the actual bus current is calculated. Compared with theoretical reference current Dynamic current deviation between .

[0153] ;

[0154] This deviation value In a physical sense, this refers to the observation residual of the disturbance observer. When the system is in a normal, contactless operating state, since the model parameters are precisely matched, It contains only measurement noise, the amplitude of which approaches zero; when external contact occurs, the torque generated by external resistance is not predicted by the model and will be directly reflected in... The dynamic current deviation serves as the primary basis for determining whether the system has entered an abnormal load region.

[0155] See attached document Figure 2 In step S20, the gradient analysis module analyzes the dynamic current deviation calculated in the previous step. Perform time-domain gradient analysis and trend assessment. This step employs a transient detection algorithm based on energy windows. Its basic physical principle is that current changes caused by thermal drift or mechanical wear have low-frequency characteristics, resulting in a minimal gradient in the time domain; while disturbances caused by rigid collisions or human-machine contact have high-frequency step characteristics, resulting in a sudden surge in the gradient. However, simple gradient calculation is highly susceptible to sensor quantization noise interference. Therefore, this invention introduces a short-time energy integration mechanism to achieve robust detection of contact events by evaluating the disturbance energy density per unit time. This process specifically includes the following sub-steps:

[0156] In sub-step S201, the discrete gradient value of the dynamic current deviation is calculated. To suppress the amplification effect of inherent high-frequency white noise in current sampling on differential operations, this embodiment abandons the simple two-point difference method and instead employs a first-order inertial filter differential operator. This operator attenuates noise above the cutoff frequency while extracting the rate of change of the signal.

[0157] Discrete gradient values The calculation is based on the following recursive formula:

[0158] ;

[0159] The physical meaning and value definitions of each symbol in the formula are as follows:

[0160] : No. The current deviation gradient value for each control cycle, in units of ;

[0161] The current dynamic current deviation input value;

[0162] The sampling period of a discrete control system (e.g.) );

[0163] : Filtering smoothing coefficient, with a value range of (0,1). This coefficient is determined by the system's target cutoff frequency. The decision is made, and the calculation formula is as follows: In practical implementation, in order to filter out PWM switching noise, It is typically set to 1 / 5 of the current loop bandwidth, for example, 100Hz to 200Hz. The value needs to be calculated precisely according to this formula.

[0164] In sub-step S202, a sliding time window is constructed, and the cumulative gradient energy is calculated. This step, in signal processing, is a sliding window integration, used to quantify the fluctuation energy of a signal within a local time range. The system establishes a length of... First-In-First-Out (FIFO) queue, storing the most recently used items in real time. The absolute value of the gradient for each period.

[0165] Gradient energy accumulation value The calculation is based on the following formula:

[0166] ;

[0167] The physical meaning and value definitions of each symbol in the formula are as follows:

[0168] The cumulative gradient energy at the current moment;

[0169] : Sliding window length (number of sampling points). Its physical value is determined based on the transient response time of the mechanical system, and is usually set as the window time length. Covering the rise time of a typical micro-collision (approximately 5 seconds) Up to 10 This ensures that the energy characteristics of a single contact event can be fully captured.

[0170] In sub-step S203, a dynamic trigger threshold based on motion state is constructed. Because the motor's current noise floor is significantly higher at high speeds due to the irregularity of the bearing balls and commutation ripple, a fixed threshold would lead to false alarms at high speeds or missed alarms at low speeds. Therefore, the system adopts a linear adaptive threshold model.

[0171] Dynamic trigger threshold The parameter determination requires a full-velocity noise calibration process (i.e., the model training process):

[0172] Control the motor to accelerate from 0 to rated speed in a step-by-step manner under no-load conditions. ;

[0173] At each speed point Record a stable gradient energy data point and calculate the noise mean at that velocity. and standard deviation ;

[0174] in accordance with Statistical criteria were used to select noise boundary points, and a basic static threshold was obtained through linear regression fitting. and speed compensation coefficient Dynamic trigger threshold The online calculation formula is as follows:

[0175] ;

[0176] : The dynamic threshold of the current control cycle;

[0177] The gradient energy noise floor limit under static conditions is taken as the mean of the measurement noise at rest plus 3 standard deviations.

[0178] Speed ​​gain coefficient, representing the linear slope of current noise as rotational speed increases, is determined by the slope term of the linear regression mentioned above;

[0179] : The absolute value of the current target angular velocity.

[0180] In sub-step S204, the logic for determining the suspected interference critical state is executed. The gradient energy is calculated in real time. With dynamic threshold Compare them.

[0181] like The system is determined to be subjected to an external transient disturbance, entering a suspected interference critical state. At this time, the system sets a flag. This flag directly triggers the subsequent active signal injection process. It's important to note that this determination only indicates the detection of an unexpected energy surge; it doesn't distinguish between a rigid collision and flexible contact. Therefore, a shutdown operation is temporarily suspended to ensure operational continuity.

[0182] like The system is determined to be in normal operating condition, and the existing control mode is maintained.

[0183] See attached document Figure 3 In step S30, the signal injection module generates and injects a composite detection signal. This step is an active identification process triggered immediately after step S20 determines that the system has entered a suspected interference critical state. The core of this invention lies in changing the limitation of traditional passive monitoring technology that relies solely on existing state data. Instead, it actively superimposes an excitation signal with specific spectral characteristics into the motor control circuit, and uses the excitation-response principle to establish a mapping relationship from microscopic electrical quantities to macroscopic mechanical impedance through physical means. This process specifically includes the following sub-steps:

[0184] In sub-step S301, the signal injection module locks the current motion state and acquires the base holding voltage. At the moment a suspected collision was detected The controller immediately suspends the current trajectory planning interpolation calculation. To prevent the robotic arm from falling due to gravity during mode switching, the system reads the current-loop PI controller's integral term output value and stores it as the base holding voltage. This voltage component physically corresponds to the balancing torque required to overcome the current gravitational load and steady-state frictional force.

[0185] In sub-step S302, a composite injection signal containing a DC preload component and an AC probe component is constructed. To accurately distinguish the material properties of the contacting object, the injected signal must simultaneously accomplish two physical tasks: eliminating mechanical transmission backlash and exciting a frequency domain impedance response. The system generates a composite voltage waveform in real time within the microprocessor based on the following formula:

[0186] ;

[0187] The physical meaning, value definition, and calibration method of each symbol in the formula are as follows:

[0188] : The target voltage command to be injected into the q-axis (torque axis) of the motor winding at the current moment;

[0189] The base holding voltage latched in step S301;

[0190] sgn : The angular velocity direction sign just before triggering detection, used to ensure that the direction of the DC preload is pointing towards the potential obstacle;

[0191] DC preload voltage amplitude. Its physical function is to ensure tight meshing of the gear teeth and eliminate interference from the nonlinear dead zone on impedance measurement. This parameter is determined through a "friction torque scanning experiment": with the motor under no-load conditions, the voltage is gradually increased until the encoder detects a micro-motion of one pulse; the voltage value at this point is recorded. This value is typically about 3% to 5% of the motor's rated voltage.

[0192] The amplitude of the AC detection signal. This value is determined according to the signal-to-noise ratio (SNR) criterion. It must ensure that the amplitude of the generated current response is at least 10 times the noise floor of the current sensor, and at the same time, it must be limited to within 10% of the rated voltage of the motor to avoid causing visible mechanical vibration.

[0193] The frequency of the AC detection signal. The selection of this frequency must satisfy two physical constraints:

[0194] First, the first natural frequency of the robotic arm must be avoided. To prevent resonance;

[0195] Second, the cutoff frequency must be lower than that determined by the electrical time constant of the motor to ensure the inductive reactance. This prevents the frequency from becoming too large and overwhelming the current signal. In this embodiment, a fixed frequency between 200Hz and 500Hz is selected.

[0196] In sub-step S303, the generated composite signal The voltage input to the q-axis of the vector control (FOC) system is applied. To ensure the probe signal is not suppressed by the feedback regulation of the current loop, the system sets the current loop PI controller to open-loop or feedforward direct-through mode. Specifically, this is achieved by cutting off the speed loop output. Directly assigned to the FOC transformation module Input port, while maintaining d-axis voltage This voltage-feedforward injection method ensures that all electromagnetic energy is directly converted into torque fluctuations in the air gap magnetic field, thereby maximizing sensitivity to external mechanical impedance.

[0197] In sub-step S304, time window control of the injection process is performed. Considering that continuous high-frequency oscillations may cause temperature rise in the motor coils or discomfort to the operator, the signal injection process is strictly limited to a short time window. Inside. Once injection begins, the system starts a high-precision timer. When At this point, regardless of whether material identification is completed, the system will forcibly exit the injection mode. The value is set to 50ms to 100ms. This duration is determined based on the full-cycle sampling requirement of signal processing, that is, at a detection frequency of 200Hz, it can ensure that at least 10 complete sine wave cycles are collected, thereby meeting the spectral resolution requirements of the subsequent DFT transformation, while being far below the human body's reaction time threshold for pain perception.

[0198] See attached document Figure 4 In step S40, the impedance analysis module performs frequency domain response extraction and phase feature decoupling. This step is the core of analyzing the physical response generated by the high-frequency excitation signal injected in step S30. Its fundamental technical principle is based on the motional impedance effect: under AC signal excitation, the motor is not merely a series circuit of resistor and inductor; the micro-vibration of its rotor generates a back electromotive force (Back-EMF). This Back-EMF is equivalent to an additional impedance component inversely proportional to the mechanical load impedance in the circuit equation. When the stiffness of the external contact object changes, the mechanical resonance characteristics change, causing a drift in the phase angle of the Back-EMF, which directly modulates the phase lag of the total loop current. This invention utilizes this physical mechanism to inversely deduce the mechanical properties of the contact material by demodulating the current phase. This process specifically includes the following sub-steps:

[0199] In sub-step S401, the current sampling data undergoes bandpass filtering and windowing preprocessing. The impedance analysis module reads the data within the signal injection time window. High-frequency sampling current sequence within To accurately extract the probe frequency component from the raw signal, which includes DC load current and PWM switching noise, the system first passes the data through a second-order Butterworth bandpass filter. The center frequency of this filter is set to be the same as the injection frequency. Strict lockout (e.g., 200Hz), passband bandwidth set to To achieve extremely high frequency selectivity, the system multiplies the filtered data sequence by a Hanning window function (Hz). Subsequently, to suppress spectral leakage caused by the truncated data length not being an integer multiple of the signal period, the system multiplies the filtered data sequence by this window. The discontinuity of the waveform is smoothed by reducing the signal amplitude at both ends of the time domain.

[0200] In sub-step S402, the real and imaginary parts of the response current are calculated using a single-frequency discrete Fourier transform (DFT) algorithm. Compared to the full-spectrum FFT, this algorithm only focuses on the injected frequency, significantly reducing the computational burden on the embedded processor. The system internally generates standard sine and cosine reference sequences that are in phase and at the same frequency as the injected signal, and performs correlation calculations with the sampled current. The cumulative real part value... Cumulative value with imaginary part The calculation is based on the following formula:

[0201] ;

[0202] ;

[0203] The physical meaning and value definitions of each symbol in the formula are as follows:

[0204] : The q-axis current sequence after preprocessing;

[0205] Hanning window function coefficients;

[0206] : Injected signal frequency;

[0207] Current sampling period (e.g.) );

[0208] Total number of points within the sampling window.

[0209] In substep S403, the absolute phase lag is calculated and the system's inherent delay compensation is performed. First, the total phase angle is calculated using the arctangent function, and then the system's inherent phase offset is subtracted. The mechanical coupling phase difference characterizing the external material properties is obtained. .

[0210] Mechanical coupling phase difference The calculation is based on the following formula:

[0211] ;

[0212] The physical meaning and value definitions of each symbol in the formula are as follows:

[0213] : Arctangent function in four quadrants;

[0214] The inherent phase offset parameter of the system at a specific frequency. This parameter is determined through an unloaded calibration process: during the robot's manufacturing phase, the control motor undergoes a signal injection test in free space without any external contact. The phase angle measured at this time consists only of the motor winding inductance, the driver dead-time effect, and the sampling transmission delay, and is recorded as a reference value. It is then stored in non-volatile memory.

[0215] In sub-step S404, the phase eigenvalue is output. Towards subsequent modules.

[0216] The correspondence between this characteristic value and the physical operating conditions is as follows:

[0217] Rigid contact condition: When the end effector of the robotic arm rests against a rigid wall, the motor rotor is physically locked and cannot generate micro-vibrations, and the back electromotive force component approaches zero. At this time, the motor is equivalent to a pure "resistive-inductive (RL)" load, and its phase angle depends only on the electrical time constant. The theoretical value approaches 0.

[0218] Flexible contact operation: When a robotic arm compresses human muscles, the flexible tissue allows the rotor to generate minute, synchronous vibrations. This vibration induces a back electromotive force (EMF) in the windings, which, superimposed on the power supply voltage in the vector diagram, alters the phase relationship between the total voltage and the total current. Because the stiffness of biological tissue is much less than that of metal, the resulting motional impedance component is significant, leading to… It exhibits a significant offset (usually greater than) Through this mechanism, the present invention successfully transforms the complex problem of tactile perception into a simple problem of electrical phase measurement.

[0219] To better understand the technical solution of this invention, the following description is based on a typical collaborative robot joint control scenario.

[0220] This embodiment applies to the third joint (elbow joint) of a 6-DOF collaborative robotic arm. This joint is driven by an integrated servo joint module, and the specific hardware parameters are as follows:

[0221] Drive motor: 400W permanent magnet synchronous motor (PMSM), number of pole pairs .

[0222] Speed ​​reduction mechanism: harmonic speed reducer, speed reduction ratio 100:1.

[0223] Sensor: 17-bit multi-turn absolute encoder (mounted on the motor side).

[0224] Controller: Servo driver based on ARM Cortex-M7 core, current loop sampling frequency 10kHz ( ), PWM switching frequency 20kHz.

[0225] Parameter calibration results:

[0226] Based on the calibration process in steps S103 and S203, the key parameters for system initialization are as follows:

[0227] Equivalent moment of inertia .

[0228] Equivalent viscous damping .

[0229] Frictional torque .

[0230] Gradient analysis sliding window length (Corresponding time window 5ms).

[0231] Dynamic threshold benchmark speed coefficient .

[0232] System inherent phase lag .

[0233] Scenario 1: Rigid collision detection.

[0234] The robotic arm performs a downward pressing motion, and the target angular velocity is...

[0235] time The end effector of the robotic arm accidentally struck the steel worktable.

[0236] Condition monitoring: Actual current In an instant Rise to Dynamic current deviation gradient The gradient energy increases sharply. The value reached 0.15, significantly higher than the dynamic threshold at the current speed. 0.025.

[0237] Signal Injection (S30): The system determines that it has entered a suspected interference state and immediately locks the base voltage. and superimposed frequency Hz, amplitude The sinusoidal detection voltage.

[0238] Impedance analysis (S40): After 50ms of data acquisition and DFT analysis, the phase characteristics of the current response were extracted. The original phase angle was calculated as follows: Subtracting inherent lag Afterwards, mechanical coupling phase difference .

[0239] Safe execution (S50): Due to (Set as) The system identifies the problem as "rigid obstruction." The controller immediately switches to current limiting mode to prevent motor overload and stops with an error message.

[0240] Scenario 2: Flexible clamping detection (human-computer interaction).

[0241] The robotic arm performs a lateral swing, with a target angular velocity.

[0242] time The robotic arm joint squeezed the operator's arm.

[0243] State monitoring (S10-S20): Due to the compliance of human muscles, the current rise is relatively gradual, but the gradient energy occurs within 10ms. The cumulative value still reaches 0.06, exceeding the threshold. This triggers the detection.

[0244] Signal injection (S30): The system injects the same 200Hz detection signal again.

[0245] Impedance analysis (S40): The spring-damped properties of muscle tissue cause a phase shift in the back electromotive force. DFT analysis shows the original phase angle is... The mechanical coupling phase difference was calculated. .

[0246] Safe execution (S50): Due to (Set as) The system determines this to be a flexible clamping action. The controller immediately increases the position loop gain. decay to and retreat in the opposite direction The pressure on the operator's arm was quickly released, and no injury was caused.

[0247] To verify the effectiveness of the material identification method based on active signal injection proposed in this invention, the following comparative experiment was conducted.

[0248] Experimental environment setup.

[0249] Experimental subject: The 6-DOF collaborative robotic arm in the above specific embodiment.

[0250] Measurement equipment: a six-dimensional force sensor (installed at the end as a reference for the true value of the collision force, not involved in control) and a high-precision oscilloscope (to monitor current and position waveforms).

[0251] Comparison group settings:

[0252] Control group (existing technology): The traditional momentum observer-based collision detection method is used, a fixed threshold is set, and the system stops abruptly upon triggering.

[0253] Experimental group (this invention): adopts the composite control strategy of gradient analysis + signal injection + impedance analysis described in this specification.

[0254] Experiment content.

[0255] At the same speed ( Two sets of tests were conducted separately:

[0256] Test A (rigidity): Impact on aluminum alloy profiles.

[0257] Test B (Flexibility): Impact a biomimetic silicone block (simulating human muscle) with a Shore hardness of 20A.

[0258] Experimental results data.

[0259] The test results are shown in Table 1, which records the average statistical data from multiple experiments. The corresponding comparison chart is shown below. Figures 5 to 6 As shown.

[0260] Table 1

[0261]

[0262] The experimental results are shown in Table 1. The method proposed in this invention can identify the properties of the collision material within 60ms by utilizing the electrical response characteristics of the motor itself without increasing the cost of an external torque sensor. Compared with existing technologies, this method significantly improves compliance with unintended human-machine contact while ensuring the response speed of rigid collision protection, and solves the problem of personnel pinching injury risk caused by the one-size-fits-all approach of the traditional current threshold method, thus having significant engineering application value.

[0263] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for preventing hand pinching in a robot servo system, characterized in that, Includes the following steps: The system monitors the operating status of the drive motor in real time. When the system is detected to be in a suspected interference state, the system suspends the normal control and acquires the base holding voltage. A composite detection signal is injected into the drive motor. The composite detection signal is constructed by superimposing a DC preload component and an AC detection component on the base holding voltage. The feedback current of the drive motor is collected, the current response component with the same frequency as the AC detection component is extracted, and the phase lag difference of the current response component relative to the AC part of the composite detection signal is calculated. The phase hysteresis difference is compared with a preset material property threshold. Based on the comparison result, the material property of the obstructing object is identified, and the corresponding graded protection strategy is executed.

2. The method for preventing hand pinching in a robot servo system according to claim 1, characterized in that, The real-time monitoring of the drive motor's operating status includes: Collect the target motion command and actual state data of the drive motor; Based on a pre-constructed rigid body dynamics model, the theoretical reference current is calculated using the target motion command. The rigid body dynamics model includes characterizations of inertial torque, viscous damping torque, and frictional torque. The difference between the actual bus current of the drive motor and the theoretical reference current is calculated to obtain the dynamic current deviation; The system is judged to have entered the suspected interference state based on the dynamic current deviation.

3. The method for preventing hand pinching in a robot servo system according to claim 2, characterized in that, The step of determining whether the system has entered the suspected interference state based on the dynamic current deviation specifically includes: Calculate the rate of change of control commands and the rate of change of position feedback separately; The ratio of the control command change rate to the position feedback change rate is calculated to obtain the blocking gradient coefficient; When the blocking gradient coefficient is greater than a preset gradient threshold, the system is determined to have entered the suspected interference state.

4. The method for preventing hand pinching in a robot servo system according to claim 1, characterized in that, The method for constructing the composite detection signal satisfies the following conditions: The base holding voltage is used to balance the gravitational load at the current moment; The direction of the DC preload component is consistent with the direction of the angular velocity at the moment before the trigger detection, and its amplitude is configured to a critical voltage value sufficient to eliminate the backlash of the transmission gear set. The AC detection component is a sinusoidal signal of a specific frequency, which is lower than the electrical cutoff frequency of the drive motor.

5. The method for preventing hand pinching in a robot servo system according to claim 1, characterized in that, In the step of injecting a composite detection signal into the drive motor, a signal injection timer is started, and an open-loop feedforward control mode is adopted, specifically including: Lock the integral term of the current loop proportional-integral controller and cut off the speed loop output; The composite detection signal is directly input to the vector control module as the torque shaft voltage command, while keeping the excitation shaft voltage command at zero. If the timer's duration exceeds a preset time window threshold, the injection of the composite detection signal will be forcibly stopped regardless of the identification result.

6. The method for preventing hand pinching in a robot servo system according to claim 1, characterized in that, The extraction of the current response component with the same frequency as the AC detection component specifically includes: The collected feedback current is subjected to bandpass filtering and windowing to obtain a preprocessed current sequence; A sine reference sequence and a cosine reference sequence with the same frequency and phase as the AC detection component are constructed inside the controller; Using a discrete transformation algorithm, the projections of the current sequence onto the sinusoidal reference sequence and the cosine reference sequence are calculated to obtain the in-phase component and the quadrature component of the current response, respectively.

7. The method for preventing hand pinching in a robot servo system according to claim 6, characterized in that, The calculation of the phase lag difference between the current response component and the AC component in the composite detection signal specifically includes: The absolute phase angle is obtained by performing a four-quadrant arctangent operation on the quadrature component and the in-phase component. The phase hysteresis difference is obtained by subtracting the system's inherent phase shift calibration constant from the absolute phase angle. The inherent phase shift calibration constant of the system is obtained in advance by injecting the composite detection signal into the drive motor under no-load conditions and measuring the phase shift.

8. The method for preventing hand pinching in a robot servo system according to claim 1, characterized in that, The step of identifying the material properties of the obstructing object based on the comparison results specifically includes: Preset rigid and flexible judgment thresholds; If the phase hysteresis difference is less than or equal to the rigidity determination threshold, the blocking object is determined to be a rigid material and identified as a rigid blocking condition. If the phase lag difference is greater than or equal to the flexibility determination threshold, the blocking object is determined to be a flexible material and identified as a flexible clamping condition.

9. The method for preventing hand pinching in a robot servo system according to claim 8, characterized in that, When the rigid blocking condition is identified, the execution of the corresponding graded protection strategy includes: Stop injecting the composite detection signal; Switch to constant torque holding mode to limit the output torque of the drive motor, restricting the output torque to a preset ratio range of the rated torque.

10. The method for preventing hand pinching in a robot servo system according to claim 8, characterized in that, When the flexible clamping condition is identified, the execution of the corresponding graded protection strategy includes: Stop injecting the composite detection signal; Perform variable stiffness impedance control to attenuate the position loop proportional gain and velocity loop differential gain to a preset ratio of the nominal value; A reverse escape trajectory is generated, driving the drive motor to move away from the obstructing object.

Images