A care robot control method, robot, storage medium, and program product

Abstract

A nursing robot control method, robot, storage medium and program product, in the method, the interaction force signal is obtained through the force sensor, and is separated into a low-frequency intention force component and a high-frequency tremor force component using a frequency threshold filter. The system judges whether there is a reverse emergency stop intention of the user by calculating the projection value of the low-frequency intention force in the motion direction, and takes the amplitude of the high-frequency tremor force component as an index of the stability degree of the user's limb control. Based on the two judgment results, the system adopts a variable damping adjustment strategy: when there is a reverse emergency stop intention, different damping control coefficients are output according to the size of the tremor intensity value, and the real-time update is updated to the admittance control model. Finally, the system calculates the target motion trajectory according to the updated model, and controls the robot to perform the corresponding action. The application is used to reduce the falling risk of the robot caused by excessive compliance, and improves the safety and stability of human-machine physical interaction.

Description

Technical Field

[0001] This application belongs to the field of intelligent control in clinical nursing, and in particular relates to a nursing robot control method, robot, storage medium and program product. Background Technology

[0002] As an automated device that assists the elderly or patients with limited mobility in their daily activities, the application value of intelligent nursing robots is becoming increasingly prominent. Early nursing robot control systems focused primarily on the robot's positioning accuracy and obstacle avoidance capabilities. When it came to direct contact with the human body, they often overlooked the complexity and unpredictability of human movement. This resulted in the robot exhibiting strong mechanical rigidity when interacting with the user, which not only reduced the user's wearing or usage comfort but also easily generated significant resistance when the user made unexpected movements, thereby increasing the risk of secondary injuries such as soft tissue contusions.

[0003] Among related technologies, an impedance control method based on force sensor feedback is widely used. This method involves installing multi-dimensional torque sensors at the robot's joints or end effectors to collect the interactive forces applied by the user to the robot in real time. Based on a preset impedance model, the interactive forces are mapped to the robot's position or velocity correction. This allows the robot to exhibit a certain degree of compliance when it senses external push or pull forces, i.e., to produce follow-up displacement or deformation like a spring. This reduces the impact load during human-robot collisions, enabling the robot to provide a smoother following experience and improving the safety of the interaction process.

[0004] When assisting semi-disabled elderly people in performing highly dynamic and high-load movements, due to their weaker body control, the force they apply to the robot's handrail is often accompanied by involuntary tremors. Furthermore, as their center of gravity rises and shifts, the magnitude and direction of the interaction force fluctuate dramatically and non-linearly. When an elderly person suddenly tends to lean backward due to insufficient leg strength or a loss of center of gravity control, their instinctive reaction is to tightly grip the robot and apply a huge pulling force to seek support. At this time, if the robot still simply follows the established impedance compliance strategy, it is very easy to misinterpret this sharp pulling force for maintaining balance as the user's intention to make the robot move backward, thus producing a compliant backward movement or reducing joint stiffness. This reduces the stability of the robot as a fixed fulcrum at critical moments when the user loses balance, increasing the risk of the user falling due to losing balance. Summary of the Invention

[0005] This application provides a nursing robot control method, robot, storage medium, and program product, which reduces the risk of falls caused by excessive compliance of the robot and improves the safety and stability of human-robot physical interaction.

[0006] Firstly, this application provides a nursing robot control method that acquires in real-time interactive force signals from force sensors mounted on the robot's end effector, as well as the robot's current actual movement speed; performs splitting processing on the interactive force signals using a preset frequency threshold filter to obtain a low-frequency intention force component with a frequency lower than a preset frequency threshold, and a high-frequency tremor force component with a frequency higher than a preset frequency threshold; uses the amplitude of the high-frequency tremor force component as a tremor intensity value characterizing the stability of the user's limb control; calculates the projection value of the low-frequency intention force component in the direction of the actual movement speed; determines that the user has a reverse emergency stop intention when the projection value is negative and the absolute value of the projection value is greater than a preset resistance force threshold; and constructs a variable damping adjustment strategy based on the tremor intensity value and the reverse emergency stop intention, and adjusts the variable damping... The first strategy includes a variable damping adjustment strategy and a second variable damping adjustment strategy. The first variable damping adjustment strategy outputs a first damping control coefficient when a reverse emergency stop intention is detected and the tremor intensity value is greater than a preset imbalance tremor threshold. The second variable damping adjustment strategy outputs a second damping control coefficient when a reverse emergency stop intention is detected and the tremor intensity value is not greater than the preset imbalance tremor threshold. The first damping control coefficient is greater than the second damping control coefficient. The first or second damping control coefficient is input into the robot's admittance control model in real time to update the virtual damping parameters in the admittance control model. Using the admittance control model with updated virtual damping parameters, the target motion trajectory of the robot is calculated based on the interaction force signal. The robot's joint actuators are controlled to make the robot move according to the target motion trajectory.

[0007] By employing the aforementioned technical solution, and acquiring interactive force signals through force sensors and performing frequency splitting processing, the system can simultaneously identify both the user's low-frequency intention force and high-frequency tremor force. By calculating the projection value of the low-frequency intention force in the direction of motion, the user's intention to reverse abruptly stop can be accurately determined. A variable damping adjustment strategy, based on tremor intensity and the intention to reverse abruptly stop, outputs a larger damping coefficient when significant tremor is detected in the user, and a smaller damping coefficient when the user's control is stable, enabling the admittance control model to adaptively adjust the virtual damping parameters. This real-time damping adjustment mechanism based on user state improves the robot's adaptability to different users and enhances the robot's safety and comfort during assistance. When the user is detected to be in an unbalanced state, a larger damping coefficient reduces the robot's response sensitivity, decreasing the risk of accidental collisions or loss of control; when the user's state is stable, a smaller damping coefficient improves the robot's following compliance, making the nursing process more natural.

[0008] In conjunction with some implementation methods of the first aspect, in some implementation methods, the mathematical expression of the admittance control model is:

[0009]

[0010] In the above formula, To preset virtual quality parameters, The virtual damping parameters are updated in real time. To preset virtual stiffness parameters, For interactive force signals, , and These are the robot's current actual position, actual velocity, and actual acceleration, respectively. , and These are the calculated target position, target velocity, and target acceleration of the robot, respectively.

[0011] By employing the above technical solution, an admittance control model is used to describe the mechanical relationship between the robot and the user through three parameters: virtual mass, virtual damping, and virtual stiffness. This gives the robot mechanical characteristics similar to a spring-damped system. By updating the virtual damping parameters in real time while keeping the virtual mass and virtual stiffness parameters constant, the robot exhibits dynamically variable damping characteristics during movement. The virtual mass parameter in the model can reduce the robot's transient response and avoid drastic acceleration changes; the virtual stiffness parameter can provide position restoring force, enhancing the robot's spatial positioning capabilities.

[0012] In conjunction with some implementation methods of the first aspect, in some implementation methods, the robot's target motion trajectory is calculated based on the interaction force signal using an admittance control model with updated virtual damping parameters. Specifically, this includes: converting the admittance control model into a difference equation in the discrete time domain and setting a sampling period; using the target position and target velocity of the previous sampling period, combined with the interaction force signal and updated virtual damping parameters of the current sampling period, substituting them into the difference equation to solve for the target acceleration of the current sampling period; integrating the target acceleration using the trapezoidal integral method to calculate the target velocity of the current sampling period; integrating the target velocity of the current sampling period using the trapezoidal integral method to calculate the target position of the current sampling period; and combining the target position and target velocity to generate the robot's target motion trajectory.

[0013] By adopting the above technical solution, the continuous-time domain admittance control model is transformed into a discrete-time domain difference equation, and the trapezoidal integral method is used for numerical solution, improving the feasibility of the control algorithm in practical systems. A complete motion trajectory planning process is formed by progressively solving for the target acceleration, target velocity, and target position. The use of the trapezoidal integral method improves the accuracy of numerical calculation and reduces truncation errors during discretization. This progressive trajectory planning method improves the continuity and smoothness of robot motion, reduces velocity jumps between adjacent sampling periods, and makes the robot's motion more rounded and fluid. By calculating the motion state of each sampling period in real time, the robot's ability to respond quickly to human movement intentions is enhanced, improving the synchronization and coordination in human-robot collaboration.

[0014] In conjunction with some implementations of the first aspect, in some implementations, the target motion trajectory of the robot is calculated based on the interactive force signal using an admittance control model with updated virtual damping parameters. Specifically, this includes: obtaining the rotation transformation matrix between the sensor coordinate system where the force sensor is located and the robot's base coordinate system; performing coordinate transformation on the interactive force signal using the rotation transformation matrix to calculate the equivalent driving force in the base coordinate system; substituting the equivalent driving force as an input into the admittance control model, and combining it with the updated virtual damping parameters to calculate the target pose of the robot's end effector in Cartesian space, where the target pose includes the target position coordinates and the target attitude angle; constructing an inverse kinematics model based on the robot's link parameters, and inputting the target pose into the inverse kinematics model for inverse kinematics calculation to obtain the target joint angles corresponding to each joint of the robot; and combining the target joint angles of each joint in a time sequence to generate the target motion trajectory.

[0015] By adopting the above technical solution, and establishing a coordinate transformation relationship between the force sensor coordinate system and the robot's base coordinate system, the mapping and transformation of interactive force signals from the local coordinate system to the global coordinate system is realized. Using the robot's inverse kinematics model, the target pose in Cartesian space is converted into target angles in joint space, establishing a mapping relationship between end effector forces and joint movements. This multi-coordinate system unified control scheme improves the accuracy of force control and enhances the robot's coordination in spatial motion. Obtaining the target angles of each joint through inverse kinematics solutions improves the robot's positional and orientation accuracy when performing complex spatial movements. This method enables the robot to make accurate spatial responses based on the forces applied by the human body, improving operational accuracy and positional tracking performance during nursing procedures.

[0016] In conjunction with some implementations of the first aspect, in some implementations, after controlling the robot's joint actuators to move the robot according to the target motion trajectory, the method further includes: acquiring the robot's current joint angle information and calculating the attitude matrix of the end effector in the base coordinate system using forward kinematics; calculating the components of the end effector load gravity in each axis of the force sensor's measurement coordinate system based on the attitude matrix and the pre-calibrated end effector load mass and centroid position; subtracting the end effector load gravity component and the preset zero-point drift offset value from the original output signal of the force sensor to obtain the corrected pure interactive force signal; and using the pure interactive force signal as the interactive force signal.

[0017] By employing the above technical solution, acquiring the robot's current joint angle information and calculating the end effector's posture matrix, combining this with pre-calibrated end-load parameters to calculate the gravity component, and then subtracting the gravity component and zero-point drift bias value from the raw force sensor signal, a more accurate and pure interactive force signal can be obtained. This compensation mechanism reduces the impact of gravity interference and sensor drift on the force signal, improves the signal-to-noise ratio of the force signal, and enables the robot to more accurately perceive the actual contact force during human-robot interaction. Because the interactive force signal is purer, the robot's accuracy in recognizing human intentions is improved, thereby enhancing the accuracy of the admittance control model in calculating the target trajectory. In actual nursing care, the robot can more accurately follow human movement, reducing trajectory deviations caused by force signal distortion and improving the smoothness and comfort of human-robot collaboration.

[0018] In conjunction with some implementations of the first aspect, in some implementations, based on the attitude matrix and the pre-calibrated end-load mass and centroid position, the components of the end-load gravity in each axis of the force sensor measurement coordinate system are calculated. Specifically, this includes: constructing a unit vector of gravitational acceleration based on the base coordinate system, and multiplying the pre-calibrated end-load mass with the unit vector of gravitational acceleration to obtain the gravity load vector in the base coordinate system; extracting the rotation matrix components from the attitude matrix and transposing the rotation matrix components to obtain the rotation transformation matrix mapping from the base coordinate system to the force sensor measurement coordinate system; performing matrix multiplication of the rotation transformation matrix with the gravity load vector to calculate the gravity components of the end-load gravity in the three force axes of the force sensor measurement coordinate system; obtaining the centroid position vector of the pre-calibrated centroid position in the force sensor measurement coordinate system; calculating the cross product of the centroid position vector and the gravity components to obtain the torque components of the end-load gravity in the three torque axes of the force sensor measurement coordinate system; and combining the gravity components and torque components to obtain the components of the end-load gravity in each axis of the force sensor measurement coordinate system.

[0019] By employing the above technical solution, a unit vector of gravitational acceleration is constructed and the gravity load vector is calculated in conjunction with the mass of the end-effector load. Coordinate transformation is then performed using the rotation matrix in the attitude matrix to calculate the force and torque components in the force sensor measurement coordinate system, achieving accurate modeling and compensation for the gravity influence of the end-effector load. This mathematical modeling method considers the influence of the end-effector load's center of mass position and accurately calculates the torque components generated by gravity through vector cross product operations, thus improving the accuracy of gravity compensation. The more accurate gravity compensation mechanism reduces systematic errors in the force signal, improves force control accuracy, and enables the robot to maintain stable force control performance under different postures. Simultaneously, because gravity compensation is more accurate, it reduces shaking caused by gravity estimation errors during robot movement, improving the smoothness of the human-robot interaction process.

[0020] In conjunction with some implementations of the first aspect, in some implementations, after using the pure interaction force signal as the interaction force signal, the method further includes: if the amplitude of the pure interaction force signal is determined to be less than a preset dead zone threshold, determining that the robot is in a non-interactive state and setting the target interaction force signal to zero; if the amplitude of the pure interaction force signal is determined to be not less than the preset dead zone threshold and less than a preset maximum safe force threshold, determining that the robot is in a normal interaction state and using the pure interaction force signal as the interaction force signal; if the amplitude of the pure interaction force signal is determined to be not less than the preset maximum safe force threshold, determining that the robot is in an overload state and triggering the robot's safety stop mode.

[0021] By adopting the above technical solution and setting dead zone thresholds and maximum safe force thresholds, a three-state force signal determination mechanism was established. State determination and signal processing are performed based on the amplitude of the pure interactive force signal. This hierarchical signal processing method reduces the impact of minor disturbances on the system and improves the stability of force control. When the force signal is less than the dead zone threshold, the target interactive force is set to zero, reducing robot jitter caused by sensor noise; when the force signal is within the normal range, the original response characteristics are maintained, ensuring the system's sensitive response to human intentions; when the force signal exceeds the safe threshold, a safe stop mode is triggered, improving system safety. This force signal processing strategy enhances the system's anti-interference capability while maintaining control sensitivity, resulting in better stability and safety for the robot during nursing care.

[0022] In a second aspect, embodiments of this application provide a robot comprising: one or more processors and a memory; the memory is coupled to one or more processors and is used to store computer program code, the computer program code including computer instructions, wherein one or more processors invoke the computer instructions to cause the system to perform the method described in the first aspect and any possible implementation thereof.

[0023] Thirdly, embodiments of this application provide a computer-readable storage medium including instructions that, when executed on a system, cause the system to perform the method described in the first aspect and any possible implementation thereof.

[0024] Fourthly, embodiments of this application provide a computer program product that, when run on a system, causes the system to execute the method described in any possible implementation of the first aspect.

[0025] One or more technical solutions provided in the embodiments of this application have at least the following technical effects or advantages:

[0026] 1. This application provides a nursing robot control method that acquires interactive force signals through a force sensor and performs frequency splitting processing, enabling simultaneous identification of the user's low-frequency intention force and high-frequency tremor force. By calculating the projection value of the low-frequency intention force in the direction of motion, the user's reverse stop intention can be accurately determined. A variable damping adjustment strategy based on tremor intensity and reverse stop intention outputs a larger damping coefficient when a large tremor is detected in the user, and a smaller damping coefficient when the user's control is stable, allowing the admittance control model to adaptively adjust the virtual damping parameters. This real-time damping adjustment mechanism based on user state improves the robot's adaptability to different users and enhances the robot's safety and comfort during assistance. When the user is detected to be in an unbalanced state, a larger damping coefficient reduces the robot's response sensitivity, decreasing the risk of accidental collisions or loss of control; when the user's state is stable, a smaller damping coefficient improves the robot's following compliance, making the nursing process more natural.

[0027] 2. This application provides a nursing robot control method. By acquiring the robot's current joint angle information and calculating the end effector posture matrix, and combining this with pre-calibrated end effector load parameters to calculate the gravity component, a more accurate and pure interactive force signal can be obtained by subtracting the gravity component and zero-point drift bias value from the original force sensor signal. This compensation mechanism reduces the impact of gravity interference and sensor drift on the force signal, improves the signal-to-noise ratio of the force signal, and enables the robot to more accurately perceive the actual contact force during human-robot interaction. Because the interactive force signal is purer, the robot's accuracy in recognizing human intentions is improved, thereby increasing the accuracy of the admittance control model in calculating the target trajectory. In actual nursing care, the robot can more accurately follow human movement, reducing trajectory deviations caused by force signal distortion and improving the smoothness and comfort of human-robot collaboration. Attached Figure Description

[0028] Figure 1 This is a flowchart illustrating a nursing robot control method in an embodiment of this application.

[0029] Figure 2 This is another flowchart illustrating a nursing robot control method in an embodiment of this application.

[0030] Figure 3 This is a schematic diagram of the physical device structure of a nursing robot control system provided in an embodiment of this application. Detailed Implementation

[0031] The terminology used in the following embodiments of this application is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. As used in the specification and appended claims of this application, the singular expressions “a,” “an,” “the,” “the,” “the,” and “this” are intended to include the plural expressions as well, unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used in this application refers to any or all possible combinations including one or more of the listed items.

[0032] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature, and in the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0033] The following example is used in conjunction with Figure 1 The following describes a nursing robot control method in an embodiment of this application:

[0034] Please see Figure 1 This is a flowchart illustrating a nursing robot control method in an embodiment of this application.

[0035] S101. Real-time acquisition of interactive force signals fed back by force sensors installed on the robot's end effector, as well as the robot's current actual movement speed;

[0036] An end effector is the mechanical interface part of a robot that directly interacts with the user or environment, such as a robot's handle, gripper, or mechanical gripper. A force sensor is a sensing device installed between the end effector and the end flange of the robot arm. It typically uses a six-dimensional force / torque sensor, capable of simultaneously measuring force along three Cartesian axes and torque about those axes. However, in this step, we mainly focus on the interactive force signal, i.e., the force vector applied by the user to the robot's end effector to guide or counteract its movement. Actual motion velocity refers to the linear velocity vector of the robot's end effector in Cartesian space at the current moment. Real-time acquisition means that the data acquisition system periodically reads sensor data and robot status at a fixed high sampling rate, ensuring extremely low response latency in the control system. During this process, the signals fed back from the force sensors typically include the user's intentional force, forces generated by limb tremors, and environmental noise. Actual motion velocity is usually calculated by reading the position encoder values ​​of each joint of the robot, combined with the robot's forward kinematics model and Jacobian matrix, or directly through inertial measurement unit (IMU) measurement.

[0037] The first approach is centralized data acquisition based on bus communication. In this approach, a six-dimensional force sensor connects to the robot controller via a high-speed industrial fieldbus. The controller sends a synchronization command in each control cycle, and the force sensor packages the force data, after internal analog-to-digital conversion and calibration matrix operations, and sends it back to the controller. Simultaneously, the controller reads the encoder position feedback values ​​from each joint servo drive via the same bus. The controller internally runs kinematic algorithms, using differential methods to perform differential calculations on the joint positions to obtain the joint angular velocities. These angular velocities are then mapped to Cartesian space using the robot's Jacobian matrix to calculate the actual linear velocity of the end effector.

[0038] The second approach combines analog signal acquisition with an external observer. In this method, the force sensor outputs a raw analog voltage signal, which is converted from analog to digital by a high-precision external data acquisition card and then input to the controller. The controller converts the voltage value into a force value based on the sensor's calibration parameters. For velocity acquisition, in addition to using encoder differentiation, an inertial measurement unit (IMU) or visual odometry (VOM) can be installed on the end effector. The controller fuses the kinematic velocity calculated by the encoder with the acceleration integral velocity measured by the IMU, and uses a Kalman filter algorithm to fuse the data, thereby obtaining a smoother and more interference-resistant estimate of the actual end effector velocity.

[0039] S102. The interaction force signal is split by a preset frequency threshold filter to obtain a low-frequency force component with a frequency lower than the preset frequency threshold and a high-frequency vibration force component with a frequency higher than the preset frequency threshold.

[0040] The preset frequency threshold filter is a digital signal processing algorithm whose core parameter is the cutoff frequency (i.e., the preset frequency threshold). This threshold is set based on the biomechanical characteristics of human movement. Generally, the frequency of a person's active movement intention is low, typically between 0Hz and 2Hz; while the frequency of pathological tremors is higher, typically between 3Hz and 12Hz. Therefore, this threshold is usually set between 2Hz and 4Hz as a dividing line between the two. Splitting processing refers to the process of decomposing a single input signal into two or more output signals of different frequency bands. The low-frequency intention force component refers to the smooth force signal that represents the user's subjective movement intention after filtering; it reflects the direction and force of the robot's movement that the user desires. The high-frequency tremor force component refers to the rapidly changing force signal that represents the user's involuntary tremors; this signal is usually considered interference, but in this method, it is used as an important basis for assessing the user's state. The key to this step is that the filter design must ensure thorough separation while minimizing signal phase delay to avoid lag in the control system.

[0041] The first approach involves using a cascaded combination of infinite impulse response (IOR) filters. Specifically, a low-pass Butterworth filter is designed with its cutoff frequency set to a preset threshold. The original interaction force signal is input into this low-pass filter, and the output is the low-frequency intention force component. Simultaneously, a high-pass Butterworth filter is designed, with its cutoff frequency also set to the same threshold. Alternatively, the calculated low-frequency intention force component can be directly subtracted from the original signal to obtain the high-frequency flutter force component. This method requires less computation and is suitable for real-time operation in embedded systems.

[0042] The second approach involves using Fast Fourier Transform (FFT) or Wavelet Transform (WFT) for frequency domain separation. Specifically, the interaction force signal sequence within a time window is decomposed using discrete wavelet decomposition, selecting appropriate wavelet basis functions. The signal is decomposed into approximation coefficients (low-frequency components) and detail coefficients (high-frequency components). Based on the decomposition level corresponding to a preset frequency threshold, the approximation coefficients are reconstructed to obtain the low-frequency intentional force component, and the detail coefficients are reconstructed to obtain the high-frequency tremor force component. This method offers better localization characteristics in the time-frequency domain and can more accurately separate non-stationary tremor signals, but its computational complexity is relatively high.

[0043] S103. The amplitude of the high-frequency tremor force component is used as the tremor intensity value to characterize the stability of the user's limb control.

[0044] The high-frequency tremor force component is the high-frequency signal separated in the previous step, typically appearing as a sinusoidal or chaotic waveform oscillating around zero. Amplitude here does not refer to an instantaneous value at a single moment, but rather to the strength of the signal vibration within a certain time window, reflecting the magnitude of energy. The tremor intensity value is a scalar value calculated based on the amplitude, used to visually represent the severity of the tremor. The stability of the user's limb control is an inverse indicator of the tremor intensity value; a higher tremor intensity value indicates less stable limb control, possibly indicating fatigue, tension, or a pathological attack; conversely, a lower value indicates stable control. This step achieves a semantic conversion from "signal" to "state characteristics." During processing, potential noise spikes in the signal need to be considered; therefore, the calculation method needs to be robust and cannot rely solely on single-point extreme values ​​for judgment.

[0045] The first method uses the sliding window root mean square (RMS) calculation method. A fixed-length time window (e.g., 200ms) is set, and this window slides forward over time. At each sampling moment, data points of all high-frequency jitter force components within the window are extracted, and the root mean square value of these data points is calculated. The formula logic is to first sum the squares of the data within the window, then take the average, and finally take the square root. The calculated RMS value is used as the current jitter intensity value. This method can effectively reflect the average energy of the jitter signal and has a smoothing effect on occasional noise spikes.

[0046] The second method is envelope detection. First, the absolute value of the high-frequency flutter force component is taken. Then, the absolute value signal is smoothed by passing it through a low-pass filter with an extremely low cutoff frequency (e.g., 0.5Hz), or the analytic envelope of the signal is extracted using Hilbert transform. The value of the resulting envelope at the current moment is taken as the flutter intensity value. This method can quickly track the changing trend of flutter intensity, and compared to the RMS method, its response speed may be faster, better reflecting the instantaneous fluctuation characteristics of flutter.

[0047] S104. Calculate the projection value of the low-frequency intentional force component in the direction of the actual motion velocity;

[0048] The low-frequency intention force component represents the user's desired direction and force of motion. The actual velocity direction represents the robot's current motion trend. The projection value is a scalar, mathematically equivalent to the dot product of two vectors. Specifically, it is the magnitude of the component of the low-frequency intention force vector in the direction of the actual velocity vector. If the projection value is positive, it means the angle between the force and velocity direction is less than 90 degrees, i.e., the user is pushing the robot to accelerate or maintain motion; if the projection value is negative, it means the angle is greater than 90 degrees, i.e., the user is applying a counterforce to try to hinder or stop the robot's movement. The absolute value represents the magnitude of this hindering force. The preset resistance force threshold is a pre-set force standard (e.g., 10N) used to distinguish between unintentional slight touches and intentional strong resistance. The physical significance of this step is to identify "braking" behavior, i.e., whether the user is actively resisting the robot's current movement.

[0049] The first method is based on dot product operations using vector algebra. First, obtain the current actual velocity vector and normalize it to obtain a unit direction vector representing the direction of motion. Then, perform a dot product operation between the low-frequency intentional force vector and this unit direction vector. The result is the projection value. If the velocity is zero or close to zero, special handling is required (such as setting the projection value to 0 or directly using the magnitude of the force) to avoid division by zero errors. This method is direct and conforms to geometric definitions.

[0050] The second approach is based on coordinate rotation transformation. A local coordinate system is constructed with the actual velocity direction as the X-axis. A rotation matrix is ​​used to transform the low-frequency intentional force vector from the robot's base coordinate system or sensor coordinate system to this local coordinate system based on the velocity direction. After the transformation, the component values ​​of the force vector on the X-axis of the local coordinate system are the desired projection values. This method, when dealing with multidimensional forces and complex trajectory relationships, can more intuitively decompose the force components in the tangential (velocity direction) and normal (steering direction), facilitating subsequent more complex motion analysis.

[0051] S105. If the projection value is determined to be negative and the absolute value of the projection value is greater than the preset resistance threshold, it is determined that the user has the intention to reverse the emergency stop.

[0052] A reverse emergency stop intention refers to a user's strong subjective desire for the robot to immediately stop its current movement, typically occurring when encountering obstacles, going the wrong way, or feeling panicked. The determination is based on two conditions: first, the projected value must be negative, geometrically meaning the force is opposite to the velocity direction—that is, the user is "pulling" the robot forward or "pushing" it backward, exhibiting braking behavior; second, the absolute value of the projected value must be greater than a preset resistance force threshold, meaning this braking behavior is not minor friction or accidental contact, but rather the application of sufficiently large force, indicating a firm intention. Only when both conditions are met simultaneously does the system determine that the user has a reverse emergency stop intention. This dual-verification mechanism is to prevent accidental triggering, ensuring the smooth operation of the robot during normal operation, and only intervening in genuine emergency or confrontational situations.

[0053] The first method uses a Boolean threshold comparison. Within each cycle of the control program, an IF-ELSE logic statement is written. First, it checks if the projected value is less than zero; if so, it checks if the absolute value of the projected value is greater than a preset resistance threshold (e.g., 15N). If both conditions are true, a Boolean variable named Stop_Intent_Flag is set to True; otherwise, it is set to False. This method is logically simple and clear, and has extremely high execution efficiency, making it a commonly used method in industrial control.

[0054] The second approach uses fuzzy logic reasoning. A fuzzy reasoning system is designed with the input variables being the "magnitude of the projection value" and the "angle between force and velocity." Fuzzy sets are defined, such as "completely opposite," "lateral," and "same direction" for the angle, and "large," "medium," and "small" for the force magnitude. Fuzzy rules are formulated, such as "if the angle is completely opposite and the force magnitude is large, then the intention to reverse stop is strong." An intention probability value between 0 and 1 is calculated through defuzzification. If this probability value exceeds a set confidence threshold (e.g., 0.8), then a reverse stop intention is determined to exist. This approach can handle boundary cases, making intention judgment more flexible and consistent with the uncertainty characteristics of human operation.

[0055] S106. Construct a variable damping adjustment strategy based on the flutter intensity value and the intention of reverse emergency stop;

[0056] A variable damping adjustment strategy is constructed based on the tremor intensity value and the intention to reverse abruptly stop. The variable damping adjustment strategy first includes a variable damping adjustment strategy and a second variable damping adjustment strategy. The first variable damping adjustment strategy outputs a first damping control coefficient when it is determined that there is an intention to reverse abruptly stop and the tremor intensity value is greater than a preset imbalance tremor threshold. The second variable damping adjustment strategy outputs a second damping control coefficient when it is determined that there is an intention to reverse abruptly stop and the tremor intensity value is not greater than the preset imbalance tremor threshold. The first damping control coefficient is greater than the second damping control coefficient.

[0057] Variable damping adjustment strategy refers to a control logic where the robot's virtual damping parameters are not fixed but dynamically changed according to real-time conditions. The first and second variable damping adjustment strategies represent two different parameter output modes. The first damping control coefficient is a large value, corresponding to a high-damping state. In this state, the robot moves heavily and responds sluggishly to force, similar to movement in a viscous liquid, primarily used to prevent loss of control. The second damping control coefficient is a small value, corresponding to a low-damping state. In this state, the robot moves easily and responds sensitively to force, primarily used for normal assistance. The preset imbalance tremor threshold is a critical value used to determine whether the user's tremor has reached a dangerous level (i.e., an imbalance state). The logic is as follows: Given that it is confirmed the user intends to stop abruptly (a reverse intention to stop exists), if the tremor is strong (greater than the threshold, indicating the user may be panicked or unbalanced), a large damping (first coefficient) is output to provide stable support; if the tremor is weak (less than the threshold, indicating the user only wants to stop but is still in control), a smaller damping (second coefficient, but usually still larger than the damping during normal walking) is output to achieve smooth deceleration. Note that the first damping coefficient must be greater than the second damping coefficient to reflect the differences in safety strategies under different risk levels.

[0058] The first approach is discrete switching control based on a state machine. A finite state machine is constructed, including states such as "normal walking," "smooth emergency stop," and "imbalance emergency stop." When a true reverse emergency stop intention is detected, the system enters the emergency stop branch. Within this branch, the current flutter intensity value is compared with a preset imbalance flutter threshold. If the flutter value is greater than the threshold, the state switches to "imbalance emergency stop," and the system directly outputs a preset first damping control coefficient constant; if the flutter value is not greater than the threshold, the state switches to "smooth emergency stop," and the system outputs a second damping control coefficient constant. This approach is simple to implement and allows for convenient parameter adjustment.

[0059] The second approach is based on smooth mapping control using a continuous function. A piecewise continuous function is designed, with tremor intensity as the independent variable and the damping coefficient as the dependent variable. Under the condition of a reverse emergency stop, this function exhibits a monotonically increasing curve (like a segment of a sigmoid function). When the tremor intensity is low, the function output value approaches the second damping control coefficient; as the tremor intensity increases and exceeds the threshold, the function output value smoothly transitions and approaches the first damping control coefficient. This approach avoids the discontinuity in control caused by abrupt parameter changes, making the damping adjustment process more delicate and natural.

[0060] S107. Input the first damping control coefficient or the second damping control coefficient into the robot's admittance control model in real time to update the virtual damping parameters in the admittance control model.

[0061] The first or second damping control coefficient is input into the robot's admittance control model in real time to update the virtual damping parameters in the admittance control model, where the mathematical expression of the admittance control model is:

[0062]

[0063] In the above formula, To preset virtual quality parameters, The virtual damping parameters are updated in real time. To preset virtual stiffness parameters, For interactive force signals, , and These are the robot's current actual position, actual velocity, and actual acceleration, respectively. , and These are the calculated target position, target velocity, and target acceleration of the robot, respectively.

[0064] Admittance control is an indirect force control method that defines a robot's dynamic response to interactive forces by simulating a second-order mechanical system consisting of mass, damping, and stiffness. The virtual damping parameter is a key term in this model, determining the rate of energy dissipation, i.e., how quickly the robot "brakes."

[0065] The first approach is based on shared memory data exchange using a real-time operating system. In the controller software architecture, the policy calculation module and the admittance control algorithm module typically run in different threads or tasks. The policy module writes the calculated first or second damping control coefficients into a global shared memory region protected by a mutex lock. At the beginning of each computation cycle, the admittance control module first accesses this shared memory, reads the latest damping coefficient value, and assigns it to the variables within the algorithm. Subsequently, utilizing this new Values ​​are combined with fixed values and Solve the differential equation for the current period.

[0066] The second approach is based on gain scheduling for parameter updates. In the control law design, the damping parameters... It is configured as a function handle that varies with external scheduling variables (in this case, tremor intensity and emergency stop intention). It is not directly stored in the controller's underlying algorithm library. Instead of storing the numerical value, it stores a pointer to the scheduling function. Each time a control operation is performed, this function is called directly. Internally, the function calculates and returns the current damping value in real time based on the current system state. This approach embeds the parameter update logic within the control law, making it suitable for nonlinear control systems with complex parameter variation patterns.

[0067] S108. Using the admittance control model with updated virtual damping parameters, calculate the target motion trajectory of the robot based on the interaction force signal.

[0068] Using the admittance control model with updated virtual damping parameters, the target motion trajectory of the robot is calculated based on the interaction force signal. One implementation method is as follows: The admittance control model is converted into a difference equation in the discrete time domain, and a sampling period is set. Using the target position and velocity from the previous sampling period, combined with the interaction force signal and updated virtual damping parameters from the current sampling period, the target acceleration for the current sampling period is calculated using the difference equation. The target acceleration is integrated using the trapezoidal integral method to calculate the target velocity for the current sampling period. The target velocity for the current sampling period is integrated using the trapezoidal integral method to calculate the target position for the current sampling period. The target position and target velocity are combined to generate the robot's target motion trajectory.

[0069] Alternatively, obtain the rotation transformation matrix between the sensor coordinate system where the force sensor is located and the robot's base coordinate system; use the rotation transformation matrix to perform coordinate transformation on the interactive force signal to calculate the equivalent driving force in the base coordinate system; substitute the equivalent driving force as an input into the admittance control model, and combine it with the updated virtual damping parameters to calculate the target pose of the robot's end effector in Cartesian space, including the target position coordinates and the target attitude angle; construct an inverse kinematics model based on the robot's link parameters, and input the target pose into the inverse kinematics model for inverse kinematics calculation to obtain the target joint angles corresponding to each joint of the robot; combine the target joint angles of each joint in a time series to generate the target motion trajectory.

[0070] Discrete-time difference equations refer to the process of slicing continuously changing differential equations along the time axis to transform them into algebraic equations solvable using addition, subtraction, multiplication, and division, in order to adapt to the digital processing characteristics of computers. The sampling period refers to the time interval between two consecutive computations performed by the computer. The trapezoidal integral method is a numerical calculation method used to estimate the total change by accumulating changes over small time intervals, such as estimating velocity from acceleration and position from velocity; its accuracy is superior to simple rectangular accumulation. A rotation transformation matrix is ​​a mathematical tool used to describe the angular deviation between two different spatial coordinate systems. The base coordinate system is a fixed reference for the robot, while the sensor coordinate system is a local reference that moves with the end effector. The inverse kinematics model is a complex set of geometric algorithms used to solve the problem of "given the hand position, find the angles of each joint" in reverse kinematics. The essence of this step is to use physical laws to simulate in the digital world and predict the robot's expected responses.

[0071] The first approach is an iterative calculation method based on time-domain numerical integration. First, the second-order differential relationship in the admittance control model is converted into a difference equation form suitable for computer processing. At the beginning of each calculation cycle, the system reads the target position and target velocity values ​​calculated in the previous cycle as the initial state, and simultaneously acquires the interaction force value fed back by the sensors at the current moment and the virtual damping value updated in the previous stage. These values ​​are substituted into the difference equation to calculate the target acceleration value at the current moment. Then, using the trapezoidal integral algorithm, the current acceleration value is weighted and averaged with the acceleration value at the previous moment, and multiplied by the sampling time to calculate the target velocity value at the current moment. Next, the trapezoidal integral algorithm is used again to integrate the target velocity value to calculate the target position value at the current moment. Finally, the calculated current position, velocity, and acceleration values ​​are combined in chronological order to generate the robot's target motion trajectory. This approach emphasizes continuous evolution in the time dimension, generating a smooth trajectory through cumulative calculations cycle by cycle.

[0072] The second approach is a pose mapping method based on spatial coordinate transformation and inverse kinematics. First, the rotational transformation matrix of the force sensor's mounting position relative to the robot's base is obtained. This matrix is ​​then used to transform the original interactive force signals collected by the sensor, mapping them from the sensor's local coordinate system to the robot's global base coordinate system, yielding the equivalent driving force value. Next, this equivalent driving force is used as input to the admittance control model, and combined with the current virtual damping parameters, the target position coordinates and target attitude angles of the robot's end effector in three-dimensional Cartesian space are directly calculated. Then, an inverse kinematics model is constructed based on the robot's mechanical link lengths and joint configuration parameters. The calculated Cartesian space target pose is input into this inverse kinematics model for reverse calculation, thus obtaining the theoretically correct target angles for each joint of the robot. Finally, the target angles of all joints are arranged and combined according to a time series to generate a target motion trajectory that can directly drive the motor. This approach emphasizes the mapping of spatial geometric relationships, ensuring the accuracy of the robot's end effector's spatial attitude.

[0073] S109. Control the robot's joint actuators to make the robot move according to the target motion trajectory.

[0074] Joint actuators typically refer to servo motors and their associated servo drives, which are the power source for the robot. The target motion trajectory has been decomposed into the target angle or angular velocity of each joint at each time step. Motion refers to the displacement of the robot entity in physical space. This step involves the underlying servo control loop, typically including a position loop, velocity loop, and current loop (torque loop). The control system needs to send commands to the actuators at extremely high frequencies (e.g., 1kHz-10kHz). The actuators, through their internal PID (proportional-integral-derivative) adjustment algorithm, control the motor current, generating electromagnetic torque to overcome friction and inertia, driving the links of the robotic arm to rotate, thus enabling the end effector to precisely follow the trajectory planned in S108. The accuracy of this step directly determines the robot's tracking performance and motion smoothness.

[0075] The first approach uses position control. The controller periodically sends the target angle (position setpoint) of each joint, calculated by the S108, to the servo driver via the bus. The servo driver internally runs a position closed-loop control algorithm, compares the received target angle with the actual angle fed back by the motor encoder, calculates the position error, and adjusts the motor output using a PID algorithm to eliminate the error. This approach has relatively low real-time requirements for the controller and relies on the underlying performance of the driver.

[0076] The second approach employs a velocity feedforward and torque compensation control mode. The controller sends not only the target position but also the target velocity and acceleration calculated by S108. Using the robot's dynamic model, the controller pre-calculates the required theoretical torque (feedforward torque) based on the target position, velocity, and acceleration, and sends it to the actuator. The actuator then superimposes this feedforward value onto its velocity or current loop. This method significantly reduces tracking lag and improves the robot's responsiveness to dynamic trajectories, especially when variable damping adjustment causes drastic velocity changes, providing a smoother motion experience.

[0077] In the above embodiments, by acquiring interactive force signals through force sensors and performing frequency splitting processing, the user's low-frequency intention force and high-frequency tremor force can be identified simultaneously. By calculating the projection value of the low-frequency intention force in the direction of motion, the user's reverse stop intention can be accurately determined. A variable damping adjustment strategy based on tremor intensity and reverse stop intention outputs a larger damping coefficient when a large tremor is detected in the user, and a smaller damping coefficient when the user's control is stable, enabling the admittance control model to adaptively adjust the virtual damping parameters. This real-time damping adjustment mechanism based on user state improves the robot's adaptability to different users and enhances the robot's safety and comfort during assistance. When the user is detected to be in an unbalanced state, a larger damping coefficient can reduce the robot's response sensitivity, reducing the risk of accidental collisions or loss of control; when the user's state is stable, a smaller damping coefficient can improve the robot's following compliance, making the nursing process more natural.

[0078] The above embodiments construct a variable damping adjustment mechanism based on user state by performing frequency splitting and state recognition on the force signal. To further improve the accuracy of force signal recognition and eliminate the influence of external interference factors, another nursing robot control method is introduced below. This method achieves accurate correction of the force sensor output signal by establishing a gravity compensation model and a zero-point drift correction mechanism. The following section combines... Figure 2 Another nursing robot control method is described in the embodiments of this application:

[0079] Please see Figure 2 This is another flowchart illustrating a nursing robot control method in an embodiment of this application.

[0080] S201. Obtain the current joint angle information of the robot and calculate the attitude matrix of the end effector in the base coordinate system through forward kinematics.

[0081] Joint angle information refers to the rotational angle values ​​of each moving joint of a robot relative to its zero or initial position. These values ​​are typically acquired in real time by encoders or angle sensors installed at the joints. Forward kinematics is a mathematical method that derives the position and orientation of a robot's end effector in space from known parameters such as link lengths and joint angles. An end effector is a tool or device installed at the end of a robot arm to perform a specific task, such as a robotic hand, gripper, or nursing handrail. The base coordinate system is a reference coordinate system fixed on the robot's base or mounting plane, serving as the absolute reference for describing the robot's overall motion. The attitude matrix is ​​a mathematical matrix describing the orientation and rotation of an object relative to the reference coordinate system, typically containing rotation matrix components. In this step, the acquisition of joint angle information is not limited to photoelectric encoders; it can also be achieved through various methods such as magnetic encoders, rotary transformers, or vision sensors. The calculated attitude matrix includes not only the orientation of the end effector but also its position information, forming a homogeneous transformation matrix. By using high-precision sensors to read the angle data of each joint in real time, and combining it with the geometric parameters of the robot body (such as link length, torsion angle, etc.), the chain rule calculation is performed using forward kinematics algorithm to accurately calculate the current spatial attitude of the end effector relative to the base. Finally, the result is output in matrix form, providing the necessary directional reference for subsequent gravity component calculation.

[0082] The specific methods and technologies for implementing this step mainly involve sensor data acquisition and kinematic calculation. The first method uses forward kinematic calculation based on the Denavit-Hartenberg (DH) parameter method. First, encoder values ​​for all joints are synchronously read from the servo driver via an industrial bus (such as EtherCAT or CAN bus), and these angle values ​​are substituted into a pre-established robot DH parameter table. The DH parameter table defines four parameters: link length, link torsion angle, link offset, and joint angle. By sequentially calculating the transformation matrix between adjacent links and multiplying all transformation matrices in order, the homogeneous transformation matrix of the end effector relative to the base coordinate system is finally obtained, from which the rotation matrix representing the attitude is extracted. The second method uses a kinematic modeling method based on the Product of Exponentials (POE) formula. This method is based on screw theory, defining the screw axis of motion for each joint and the end effector pose in the initial state, and using matrix exponential mapping to map joint angles into rigid body motion. Compared to the DH parameter method, the POE formula avoids parameter singularity issues when dealing with robots with special geometries, and its geometric meaning is more intuitive. After the system reads the joint angles, it can directly substitute them into the exponential product formula to perform matrix operations and obtain the current pose matrix.

[0083] S202. Based on the attitude matrix and the pre-calibrated end load mass and center of mass position, calculate the components of the end load gravity in each axis of the force sensor measurement coordinate system.

[0084] Based on the attitude matrix and the pre-calibrated end-load mass and centroid position, the components of the end-load gravity in each axis of the force sensor measurement coordinate system are calculated. Specifically, this includes: constructing a unit vector of gravitational acceleration based on the base coordinate system, and multiplying the pre-calibrated end-load mass with the unit vector of gravitational acceleration to obtain the gravity load vector in the base coordinate system; extracting the rotation matrix components from the attitude matrix and transposing them to obtain the rotation transformation matrix mapping from the base coordinate system to the force sensor measurement coordinate system; performing matrix multiplication between the rotation transformation matrix and the gravity load vector to calculate the gravity components of the end-load gravity in the three force axes of the force sensor measurement coordinate system; obtaining the centroid position vector of the pre-calibrated centroid position in the force sensor measurement coordinate system; calculating the cross product of the centroid position vector and the gravity components to obtain the torque components of the end-load gravity in the three torque axes of the force sensor measurement coordinate system; and combining the gravity components and torque components to obtain the components of the end-load gravity in each axis of the force sensor measurement coordinate system.

[0085] The end-effector load mass refers to the total mass of all attachments and the grasped object installed at the force sensor end; this parameter is typically obtained through offline calibration or precision weighing. The center of mass position refers to the geometric position vector of the end-effector load's center of mass relative to the force sensor's measurement center. The force sensor's measurement coordinate system is a coordinate system fixed to the force sensor itself and moves with the robot; its axes typically correspond to the sensor's sensitive axes. The gravity load vector refers to the force vector generated solely by gravity in the base coordinate system, directed vertically downwards. The rotation transformation matrix is ​​a mathematical matrix used to transform a vector from one coordinate system to another; here, it specifically refers to the mapping from the base coordinate system to the sensor coordinate system. The cross product is a vector operation used to calculate the product of the lever arm and the force to obtain the torque. In this step, the end-effector load is not limited to a single tool but can include multiple combined grippers; its center of mass position and mass can be estimated online using a parameter identification algorithm. The calculation process considers not only static gravity but also the influence of inertial forces on the sensor in conjunction with a dynamic model. The core of this step is to project the constant gravity vector onto the dynamically changing sensor coordinate system through the inverse transformation or transpose transformation of the attitude matrix, thereby accurately resolving the spurious force and torque components generated by gravity on each sensitive axis of the sensor.

[0086] The specific methods and techniques for implementing this refined technical solution mainly involve spatial vector transformation and matrix operations. The first method is a projection calculation method based on the transpose of a rotation matrix. First, a gravity acceleration vector (usually [0, 0, -g]) is constructed in the base coordinate system, and multiplied by the calibrated load mass to obtain the gravity load vector. Next, the rotation component of the attitude matrix obtained in step S201 is extracted, and the transpose matrix of this rotation matrix is ​​obtained using the properties of orthogonal matrices. This transpose matrix represents the rotation transformation from the base coordinate system to the sensor coordinate system. The transpose matrix is ​​multiplied by the gravity load vector to obtain the gravity component in the sensor coordinate system. Subsequently, the calibrated centroid position vector is read, and the cross product of the centroid vector and the gravity component is calculated using vector product operations to obtain the torque component. Finally, the three-dimensional force component and the three-dimensional torque component are combined and output. The second method is an attitude transformation calculation method based on quaternions. The attitude matrix is ​​converted into a unit quaternion representation, and the gravity vector in the base coordinate system is rotated using a quaternion rotation operator. Quaternion operations are less computationally intensive than matrix operations and avoid gimbal lock-up issues. The gravity vector in the sensor coordinate system is directly obtained through quaternion multiplication, and then torque is calculated by combining this with the center of mass position. This method is more efficient in embedded controllers.

[0087] S203. Subtract the end load gravity component and the preset zero-point drift offset value from the original output signal of the force sensor to obtain the corrected pure interactive force signal.

[0088] The raw output signal of the force sensor refers to the voltage or digital value directly output by the six-dimensional force / torque sensor before any algorithmic processing. This includes the actual interaction force, gravity component, the sensor's own zero-point drift, and noise. The preset zero-point drift bias value refers to the non-zero output value of the sensor under no external force and no load gravity (or after gravity is removed through a specific algorithm). This is usually caused by temperature changes, electronic component aging, or mechanical stress release. The pure interaction force signal refers to the force signal reflecting only the actual contact between the external environment and the robot's end effector after eliminating the influence of gravity and zero-point drift. The calibration process is an algebraic subtraction operation aimed at restoring the true physical interaction state. In this step, the zero-point drift bias value can be a fixed value or a dynamically compensated value that varies with temperature or time. The calibrated signal contains not only forces in three directions but also torques in three directions. This step, through decoupling at the software algorithm level, peels away the layers of mixed signals "seen" by the sensor, eliminating the masking effect of gravity and system errors. This allows the subtle human-computer interaction forces to be clearly extracted, providing a reliable data foundation for subsequent high-precision force control.

[0089] The specific methods and techniques for implementing this step mainly involve signal processing and data fusion. The first method is a combination of direct algebraic subtraction and low-pass filtering. The system first reads six channels of raw data from the force sensor from the data acquisition card, and simultaneously calls the six-dimensional gravity / torque components calculated in step S202, as well as the zero-point bias constant stored in non-volatile memory. A point-by-point subtraction operation is performed in the processor: the original signal is subtracted from the gravity component, and then the zero-point bias is subtracted. To remove high-frequency noise, the result after subtraction is smoothed using a Butterworth low-pass filter or a moving average filter, ultimately outputting a pure force signal. The second method is a dynamic compensation method based on an observer. A state observer is designed, treating the gravity component and zero-point drift as state variables or disturbance inputs of the system. An observer error feedback loop is constructed using the robot's dynamic model and sensor measurement equations. The observer not only calculates the influence of gravity in real time but also dynamically tracks the slow changing trend of the zero-point drift. The original signal is directly corrected through the observer's output. This method has better adaptability to handling zero-point drift that varies with temperature and can obtain a more stable and pure signal.

[0090] S204. Use the pure interaction force signal as the interaction force signal.

[0091] The robot is determined to be in a non-interactive state if the amplitude of the pure interaction force signal is less than a preset dead zone threshold, and the target interaction force signal is set to zero. If the amplitude of the pure interaction force signal is not less than the preset dead zone threshold and is less than the preset maximum safe force threshold, the robot is determined to be in a normal interaction state, and the pure interaction force signal is used as the interaction force signal. If the amplitude of the pure interaction force signal is not less than the preset maximum safe force threshold, the robot is determined to be in an overload state, and the robot's safety stop mode is triggered.

[0092] The interactive force signal is the force vector data ultimately input to the robot's admittance controller or impedance controller, directly determining the robot's motion response. A preset dead zone threshold is a set lower limit for force value, used to filter out minor noise or unintentional slight touches; only signals exceeding this value are considered valid interactions. A preset maximum safe force threshold is a set upper limit for force value, used to prevent excessive external forces from damaging the robot or injuring the user. A non-interactive state means the robot determines it is not currently under effective human manipulation and should maintain its current posture or execute a predetermined trajectory. A normal interactive state means the robot senses external forces within a reasonable range and should adjust its movement according to the magnitude and direction of the force. An overload state means the external force exceeds the safety limit, and the system must take emergency measures. The dead zone threshold must be less than the maximum safe force threshold. In this step, the state determination logic is based not only on the force amplitude but also on a comprehensive judgment combining the force's rate of change or duration. The process of using a clean signal as the interactive force signal is actually a signal threshold filtering and safety verification process, ensuring the validity and safety of the control system input and preventing malfunctions and accidents.

[0093] The specific methods and techniques for implementing this step mainly involve logical judgment and state machine management. The first method is a conditional branching method based on multi-level threshold comparison. The Euclidean norm (i.e., the magnitude of the resultant force) of the pure interactive force signal is calculated, and it is numerically compared with the dead-zone threshold and the maximum safe force threshold stored in the configuration parameters. The program uses an IF-ELSE structure: if the resultant force is less than the dead-zone threshold, all components of the output interactive force signal are forcibly set to zero, and the system flag is set to "non-interactive"; if the resultant force is between the two, the signal is passed directly, and the flag is set to "normal interactive"; if the resultant force is greater than the safe threshold, an interrupt signal is triggered, the flag is set to "overload," and the emergency stop subroutine is called. The second method is a state determination method based on fuzzy logic. The magnitude of the force is defined as a fuzzy variable, and fuzzy sets such as "minimal," "moderate," and "excessive" and their membership functions are set. The pure force signal is input into the fuzzy inference system, and the output weight is calculated according to the preset fuzzy rule base (e.g., "if the force is minimal, output zero," "if the force is moderate, maintain output"). The final interaction force signal coefficients (between 0 and 1) are obtained through defuzzification. Compared with hard threshold determination, this method can achieve a smooth transition between states and avoid abrupt changes in control signals at threshold critical points.

[0094] In the above embodiments, by acquiring the robot's current joint angle information and calculating the end effector posture matrix, and combining this with pre-calibrated end effector load parameters to calculate the gravity component, and then subtracting the gravity component and zero-point drift bias value from the raw force sensor signal, a more accurate and pure interactive force signal can be obtained. This compensation mechanism reduces the impact of gravity interference and sensor drift on the force signal, improves the signal-to-noise ratio of the force signal, and enables the robot to more accurately perceive the actual contact force during human-robot interaction. Because the interactive force signal is purer, the robot's accuracy in recognizing human intentions is improved, thereby increasing the accuracy of the admittance control model in calculating the target trajectory. In actual nursing care, the robot can more accurately follow human movement, reducing trajectory deviations caused by force signal distortion, and improving the smoothness and comfort of human-robot collaboration.

[0095] The system in the embodiments of this invention is described below from the perspective of hardware processing. Please refer to [link / reference needed]. Figure 3 This is a schematic diagram of the physical device structure of a nursing robot control system provided in an embodiment of this application.

[0096] It should be noted that, Figure 3 The structure of the system shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of the present invention.

[0097] like Figure 3 As shown, the system includes a Central Processing Unit (CPU) 301, which can perform various appropriate actions and processes based on a program stored in Read-Only Memory (ROM) 302 or a program loaded from storage portion 308 into Random Access Memory (RAM) 303, such as executing the methods described in the above embodiments. The RAM 303 also stores various programs and data required for system operation. The CPU 301, ROM 302, and RAM 303 are interconnected via a bus 304. An Input / Output (I / O) interface 305 is also connected to the bus 304.

[0098] The following components are connected to I / O interface 305: input section 306 including a camera, infrared sensor, etc.; output section 307 including a liquid crystal display (LCD) and speakers, etc.; storage section 308 including a hard disk, etc.; and communication section 309 including a network interface card such as a LAN (Local Area Network) card and a modem, etc. Communication section 309 performs communication processing via a network such as the Internet. Drive 310 is also connected to I / O interface 305 as needed. Removable media 311, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 310 as needed so that computer programs read from them can be installed into storage section 308 as needed.

[0099] In particular, according to embodiments of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present invention include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing computer programs for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 309, and / or installed from removable medium 311. When the computer program is executed by central processing unit (CPU) 301, it performs the various functions defined in the present invention.

[0100] It should be noted that the computer-readable medium shown in the embodiments of the present invention can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), flash memory, optical fiber, portable compact disc read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. In the present invention, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, wherein a computer-readable computer program is carried. The transmitted data signal can take many forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof.

[0101] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. Each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, may be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0102] In another aspect, the present invention also provides a computer-readable storage medium, which may be included in the system described in the above embodiments; or it may exist independently and not assembled into the system. The storage medium carries one or more computer programs that, when executed by a processor of a system, cause the system to implement the methods provided in the above embodiments.

[0103] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit it. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of this application.

[0104] As used in the above embodiments, depending on the context, the term "when..." can be interpreted as "if...", "after...", "in response to determining...", or "in response to detecting...". Similarly, depending on the context, the phrase "when determining..." or "if (the stated condition or event) is interpreted as "if determining...", "in response to determining...", "when (the stated condition or event) is detected", or "in response to detecting (the stated condition or event)".

[0105] In the above embodiments, implementation can be achieved entirely or partially through software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented entirely or partially in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic, digital subscriber line) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid-state drive), etc.

[0106] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. This program can be stored in a computer-readable storage medium, and when executed, it can include the processes described in the above method embodiments. The aforementioned storage medium includes various media capable of storing program code, such as ROM or random access memory (RAM), magnetic disks, or optical disks.

Claims

1. A method for controlling a nursing robot, characterized in that, include: The system acquires interactive force signals from force sensors mounted on the robot's end effector in real time, as well as the robot's current actual speed. The interaction force signal is split by a preset frequency threshold filter to obtain a low-frequency intention force component with a frequency lower than the preset frequency threshold and a high-frequency vibration force component with a frequency higher than the preset frequency threshold. The amplitude of the high-frequency tremor force component is used as the tremor intensity value to characterize the stability of the user's limb control; Calculate the projection value of the low-frequency intentional force component in the direction of the actual motion velocity; If the projection value is determined to be negative and the absolute value of the projection value is greater than a preset resistance threshold, it is determined that the user has the intention to reverse abruptly stop. A variable damping adjustment strategy is constructed based on the tremor intensity value and the reverse emergency stop intention. The variable damping adjustment strategy first includes a variable damping adjustment strategy and a second variable damping adjustment strategy. The first variable damping adjustment strategy outputs a first damping control coefficient when it is determined that the reverse emergency stop intention exists and the tremor intensity value is greater than a preset imbalance tremor threshold. The second variable damping adjustment strategy outputs a second damping control coefficient when it is determined that the reverse emergency stop intention exists and the tremor intensity value is not greater than the preset imbalance tremor threshold. The first damping control coefficient is greater than the second damping control coefficient. The first damping control coefficient or the second damping control coefficient is input into the admittance control model of the robot in real time to update the virtual damping parameters in the admittance control model. Using the admittance control model with updated virtual damping parameters, the target motion trajectory of the robot is calculated based on the interaction force signal; The joint actuators of the robot are controlled to make the robot move according to the target motion trajectory.

2. The method according to claim 1, characterized in that, The mathematical expression for the admittance control model is: In the above formula, the To preset virtual quality parameters, the For the virtual damping parameters updated in real time, the To preset virtual stiffness parameters, the The interactive force signal, the The above and stated These are the robot's current actual position, actual speed, and actual acceleration, respectively. The above and stated These are the calculated target position, target velocity, and target acceleration of the robot, respectively.

3. The method according to claim 1, characterized in that, The admittance control model utilizing the updated virtual damping parameters calculates the robot's target motion trajectory based on the interaction force signal, specifically including: The admittance control model is converted into a difference equation in the discrete time domain, and the sampling period is set. Using the target position and target velocity from the previous sampling period, combined with the interaction force signal and the updated virtual damping parameters from the current sampling period, the target acceleration for the current sampling period is calculated by substituting them into the difference equation. The target acceleration is integrated using the trapezoidal integral method to calculate the target velocity in the current sampling period; The target position in the current sampling period is calculated by integrating the target velocity in the current sampling period using the trapezoidal integral method. The target position and the target velocity are combined to generate the target motion trajectory of the robot.

4. The method according to claim 1, characterized in that, The admittance control model utilizing the updated virtual damping parameters calculates the robot's target motion trajectory based on the interaction force signal, specifically including: Obtain the rotation transformation matrix between the sensor coordinate system where the force sensor is located and the base coordinate system of the robot; The rotation transformation matrix is ​​used to perform coordinate transformation on the interactive force signal, and the equivalent driving force in the base coordinate system is calculated. The equivalent driving force is substituted into the admittance control model as an input, and the updated virtual damping parameters are combined to calculate the target pose of the robot end effector in Cartesian space. The target pose includes the target position coordinates and the target attitude angle. An inverse kinematics model is constructed based on the link parameters of the robot, and the target pose is input into the inverse kinematics model for inverse kinematics calculation to obtain the target joint angles corresponding to each joint of the robot. The target joint angles of each of the joints are combined in a time sequence to generate the target motion trajectory.

5. The method according to claim 1, characterized in that, After the joint actuators controlling the robot move the robot according to the target motion trajectory, the method further includes: Obtain the current joint angle information of the robot, and calculate the attitude matrix of the end effector in the base coordinate system using forward kinematics; Based on the attitude matrix and the pre-calibrated end load mass and center of mass position, the components of the end load gravity in each axis of the force sensor measurement coordinate system are calculated. The corrected pure interactive force signal is obtained by subtracting the end load gravity component and the preset zero-point drift offset value from the original output signal of the force sensor. The pure interaction force signal is used as the interaction force signal.

6. The method according to claim 5, characterized in that, The step of calculating the components of the end-load gravity along each axis of the force sensor's measurement coordinate system based on the attitude matrix and the pre-calibrated end-load mass and center of mass position specifically includes: A unit vector of gravitational acceleration is constructed based on the base coordinate system, and the pre-calibrated end load mass is multiplied by the unit vector of gravitational acceleration to obtain the gravitational load vector in the base coordinate system; Extract the rotation matrix components from the attitude matrix and transpose the rotation matrix components to obtain the rotation transformation matrix that maps from the base coordinate system to the force sensor measurement coordinate system; The rotation transformation matrix and the gravity load vector are multiplied together to calculate the gravity components of the end load gravity along the three force axes in the force sensor measurement coordinate system. Obtain the pre-calibrated centroid position vector in the force sensor measurement coordinate system; Calculate the cross product of the centroid position vector and the gravity component to obtain the torque components of the end load gravity along the three torque axes in the force sensor measurement coordinate system; The gravity component and the torque component are combined to obtain the components of the end load gravity in each axis of the force sensor measurement coordinate system.

7. The method according to claim 5, characterized in that, After using the pure interaction force signal as the interaction force signal, the method further includes: If the amplitude of the pure interaction force signal is determined to be less than the preset dead zone threshold, the robot is determined to be in a non-interactive state, and the target interaction force signal is set to zero. If the amplitude of the pure interaction force signal is not less than the preset dead zone threshold and is less than the preset maximum safe force threshold, the robot is determined to be in a normal interaction state, and the pure interaction force signal is used as the interaction force signal. If the amplitude of the pure interactive force signal is determined to be not less than the preset maximum safe force threshold, the robot is determined to be in an overload state, and the robot's safe stop mode is triggered.

8. A nursing robot control system, characterized in that, The system includes: One or more processors and a memory; the memory is coupled to the one or more processors, the memory being used to store computer program code, the computer program code including computer instructions, the one or more processors invoking the computer instructions to cause the system to perform the method as described in any one of claims 1-7.

9. A computer-readable storage medium comprising instructions, characterized in that, When the instructions are executed on the system, the system performs the method as described in any one of claims 1-7.

10. A computer program product, characterized in that, When the computer program product is run on the system, the system performs the method as described in any one of claims 1-7.

Images