Ultrasound robot compliant interaction control method, system and storage medium

Abstract

The application discloses a kind of ultrasonic robot compliant interaction control method, system and storage medium in the technical field of medical robot, the method includes the steps of constructing six-axis robot dynamics model, dynamic gravity compensation is carried out to end load, switches between zero force guide mode and compliant follow-up mode, introduces end displacement correction, embeds sliding mode variable structure compensation, coupling mapping etc., the program of the system and storage medium is used to realize the above-mentioned method.The application overcomes the defects such as insufficient compliant control precision of existing ultrasonic robot, poor man-machine interaction experience, insufficient control robustness, unstable imaging quality etc., it realizes high-precision gravity compensation by constructing refined six-axis robot dynamics model, designs multimodal adaptive impedance control framework, introduces sliding mode variable structure compensation, and realizes the depth multimodal coupling of visual servo and force perception, to realize high safety, high reliability of autonomous ultrasonic scanning in dynamic clinical environment.

Description

Technical Field

[0001] This invention relates to the field of medical robot technology, specifically to a compliant interactive control method, system, and storage medium for an ultrasound robot. Background Technology

[0002] In the context of modern precision medicine and remote diagnosis, ultrasound robot-assisted scanning technology has become a core means to address the uneven distribution of medical resources and reduce the risk of occupational injuries to physicians. However, ultrasound scanning is essentially a precise and dynamic physical interaction process. The robot not only needs to drive the probe to track its trajectory on a complex three-dimensional anatomical surface, but also needs to maintain a stable and appropriate contact force at all times to ensure image quality.

[0003] Traditional industrial robot control strategies are typically based on high-rigidity position control. This "rigid connection" characteristic exhibits extremely poor environmental adaptability when facing human targets with physiological rhythms (such as breathing fluctuations and heartbeat) or unpredictable movements (such as minor adjustments in patient position or sudden tremors). If the system lacks effective compliant interaction logic, the probe is highly susceptible to hard collisions with the subject, which can cause discomfort and even tissue damage to the subject, and may also damage the expensive ultrasonic transducer.

[0004] In the field of rehabilitation robotics, such as lower limb rehabilitation systems for bedridden patients, compliant control theory based on dynamic models has begun to be introduced to improve the active participation and safety of human-machine interaction. However, in the specific field of ultrasound scanning, the translation of rehabilitation theory still faces significant technical bottlenecks. First, ultrasound scanning requires extremely high precision in sensing the force at the end effector, typically requiring Newton-level force compensation within millimeter-level displacements. However, during the spatial posture changes of the robotic arm, the gravity vector of the end effector load (including the ultrasound probe, coupling agent, and six-dimensional force sensor) changes in real time with the posture. If this gravity deviation is not carefully modeled and compensated online, it will seriously contaminate the real contact force signal, causing the compliant controller to generate erroneous feedback actions, or even leading to system instability.

[0005] Secondly, most existing ultrasound robotic systems remain in a passive execution phase, lacking admittance control or "zero-force guidance" modes similar to those found in rehabilitation robots. This means that during auxiliary calibration of the scan initiation point or interactive teaching in complex areas, physicians must overcome the massive physical inertia and joint friction of the robotic arm, resulting in a stiff operating feel and difficulty in achieving sensitive clinical guidance. Simultaneously, human tissue exhibits significant nonlinear stiffness and viscoelastic characteristics, making it difficult to achieve a balance between the two distinct modes of "free-space movement" and "skin surface scanning" using simple steady impedance parameters. In special scenarios requiring high-frequency pressure feedback, such as ablation surgery, existing solutions often fail to maintain acoustic coupling consistency in highly dynamic environments due to system damping overload or dynamic response lag.

[0006] Furthermore, the path planning and force control of existing ultrasonic robot systems operate independently, lacking deep integration of visual and force information, and thus cannot simultaneously achieve accurate trajectory tracking and compliant surface adhesion on three-dimensional anatomical surfaces.

[0007] The aforementioned shortcomings deserve to be addressed. Summary of the Invention

[0008] To overcome the shortcomings of existing ultrasound robots, such as insufficient compliant control precision, poor human-computer interaction experience, insufficient control robustness, and unstable imaging quality, this invention provides an ultrasound robot compliant interactive control method, system, and storage medium. It achieves high-precision gravity compensation by constructing a refined six-axis robotic arm dynamic model, designs a multimodal adaptive impedance control framework, introduces sliding mode variable structure compensation, and realizes deep multimodal coupling of visual servoing and force admittance, thereby achieving highly safe and reliable autonomous ultrasound scanning in dynamic clinical environments.

[0009] The technical solution of this invention is as follows:

[0010] A compliant interactive control method for an ultrasonic robot, characterized by comprising the following steps:

[0011] Step S1: Construct a six-axis robotic arm dynamic model to calculate the nonlinear torque generated by the end effector of the ultrasonic robot during spatial motion in real time;

[0012] Step S2: Based on the dynamic model of the six-axis robotic arm, perform dynamic gravity compensation on the end load consisting of the ultrasonic probe and the six-dimensional force sensor mounted on the end of the robotic arm, and extract pure end contact force data.

[0013] Step S3: Based on the amplitude of the obtained pure end contact force, adaptively switch between zero-force guiding mode and compliant follow-up mode, and smoothly interpolate the impedance parameters during the switching transition process.

[0014] Step S4: In compliant follower mode, the end displacement correction is generated by the second-order differential equation of admittance control; a sliding mode variable structure compensation term is embedded in the admittance control loop to cope with the nonlinear mechanical disturbances during the ultrasonic scanning process.

[0015] Step S5: Establish a multimodal coupling architecture. Decouple the tangential path constraint command generated by image-based visual servoing and the normal directional admittance correction amount generated in step S4 through an interaction matrix in a unified joint space to drive the robotic arm to perform adaptive ultrasound scanning.

[0016] According to the present invention based on the above scheme, the feature is that, in step S1, the robot arm reset posture is used as the global reference initial zero position, the Z-axis of each joint is set as the rotation axis, the X-axis is defined on the common perpendicular line of adjacent axes, and a homogeneous transformation matrix chain from the base to the end probe is constructed by calculating the link length and offset distance between the joints.

[0017] Furthermore, a dynamic model of the six-axis robotic arm is established based on the improved DH parameters. The equations of the six-axis robotic arm dynamic model are as follows:

[0018] ;

[0019] in, Here is the joint space mass inertia matrix. The matrix of Coriolis force and centrifugal force. The vector of the gravity term. Here is the joint friction force matrix. For the driving torque vector, , , These are joint angle, angular velocity, and angular acceleration, respectively.

[0020] According to the present invention based on the above scheme, the dynamic gravity compensation formula in step S2 is:

[0021] ;

[0022] in, To compensate for the pure end contact force, The raw force data collected by the six-dimensional force sensor. This is the real-time rotation matrix from the base coordinate system to the sensor coordinate system. The gravity vector of the end load. This is the zero-drift bias of the sensor.

[0023] According to the present invention based on the above scheme, the feature is that, in step S3, a safety threshold is preset, and it is determined whether the pure end contact force exceeds the safety threshold: when the pure end contact force is lower than the safety threshold, the ultrasonic robot is in a zero-force guidance mode, the virtual mass and virtual damping parameters are adjusted to a minimum value, and the robotic arm is made to have high transparency through active dynamic compensation, so that the ultrasonic robot can be driven to move flexibly by applying a guiding force.

[0024] When the pure end contact force reaches the safety threshold, the ultrasonic robot enters a compliant follow-up mode and increases the virtual damping parameter to absorb the kinetic energy generated by the human body's breathing fluctuations.

[0025] According to the present invention based on the above scheme, the characteristic is that, in step S3, the impedance parameters during the switching transition process are smoothly interpolated using an S-type logic function, and the parameter smoothing transition expression of the S-type logic function is:

[0026] ;

[0027] in, For smooth interpolation of impedance parameters, It is an S-type logic function. These are the impedance parameter values ​​corresponding to the zero-force guided mode. These are the impedance parameter values ​​corresponding to the compliant follower mode. To compensate for the pure end contact force, This is a preset safety threshold.

[0028] According to the present invention based on the above scheme, the characteristic is that, in step S4, the admittance control second-order differential equation is:

[0029] ;

[0030] in, To compensate for the pure end contact force, , , These are the virtual mass parameter matrix, virtual damping parameter matrix, and virtual stiffness parameter matrix, respectively. , , These are the desired position, velocity, and acceleration at the end point, respectively. , , These represent the position, velocity, and acceleration of the reference trajectory, respectively.

[0031] According to the present invention based on the above scheme, the process of embedding a sliding mode variable structure compensation term in the admittance control loop in step S4 specifically includes:

[0032] (1) Constructing the force tracking error model: ,in, To compensate for the pure end contact force, For target contact force;

[0033] (2) Construct a linear switching surface model based on force error and its rate of change: , where λ is the sliding surface parameter;

[0034] (3) Use a continuous saturation function The sign function is replaced to suppress high-frequency chattering, and the compensation control law is as follows: ,in For sliding mode gain, For boundary layer thickness parameters, For robustness term gain;

[0035] (4) The compensation control law is embedded as a disturbance compensation term into the second-order differential equation of the admittance control to form the modified admittance model.

[0036] On the other hand, an ultrasonic robot compliant interaction control system is characterized by comprising the following components for implementing the aforementioned ultrasonic robot compliant interaction control method:

[0037] The hardware platform includes a six-axis robotic arm, a high-frequency linear array ultrasonic probe, a six-dimensional force sensor, a vision sensor, and a central control workstation. The six-dimensional force sensor is integrated between the ultrasonic probe and the end flange of the six-axis robotic arm.

[0038] The dynamic modeling and gravity compensation module is used to establish a dynamic model of a six-axis robotic arm and calculate the gravity vector component of the end-effector load online based on real-time feedback data from the joint encoder. It removes the influence of gravity from the raw sensor data and outputs a pure end-effector contact force.

[0039] The multi-modal adaptive impedance control module is used to adaptively switch between zero-force guiding mode and compliant follow-up mode based on the amplitude of the pure end contact force, and to smoothly transition the impedance parameters.

[0040] The admittance control and sliding mode compensation module is used to execute the second-order differential equation of admittance control to calculate the end displacement correction, and to enhance the robustness of the system to nonlinear mechanical disturbances by embedding sliding mode variable structure compensation terms.

[0041] The visual servoing and multimodal coupling module is used to decouple the tangential path constraints and admittance force corrections generated by visual servoing through an interaction matrix, and then execute them uniformly in the joint space to drive the robotic arm to complete adaptive ultrasound scanning.

[0042] Thirdly, a computer-readable storage medium having a computer program stored thereon, characterized in that, when the computer program is executed by a processor, it implements the steps of the above-described ultrasonic robot compliant interactive control method.

[0043] According to the above-described solution, the beneficial effects of this invention are as follows:

[0044] (1) This invention overcomes the shortcomings of existing ultrasonic robot control technology, which is that the end gravity deviation contaminates the contact force signal and leads to insufficient compliance control accuracy. Through a refined six-axis robotic arm dynamic model and a real-time gravity compensation algorithm, the end load gravity deviation is eliminated online with a millisecond-level response frequency, enabling the system to sense minute contact force changes as low as 0.1N, providing a reliable physical basis for high-precision compliance control.

[0045] (2) In the zero-force guidance mode, the physician only needs to apply a very small guiding force of no more than 5N to flexibly drive the heavy robotic arm end to locate the scanning start point, transforming the heavy physical labor into lightweight interactive guidance, greatly reducing the operation threshold and physical consumption of ultrasound physicians.

[0046] (3) The present invention overcomes the shortcomings of existing ultrasonic robot control technology in terms of insufficient control robustness and inability to cope with high dynamic nonlinear disturbances. The sliding mode variable structure compensation term embedded in the present invention significantly enhances the robustness of the control system to high dynamic nonlinear environments such as ablation surgery, limits the tracking deviation of time-varying force to the millinewton range, and ensures that the ultrasonic cross-sectional image has a constant signal-to-noise ratio throughout the entire examination cycle. Compared with pure admittance control without embedded sliding mode compensation, the root mean square error of force tracking is reduced by about 65%.

[0047] (4) The deep multimodal coupling of visual servoing and force admittance of the present invention enables the system to simultaneously meet the requirements of trajectory tracking accuracy and compliant adhesion on the three-dimensional anatomical surface, realize the high standardization of the scanning process, significantly improve the repeatability and work efficiency of clinical diagnosis, and has extremely high social benefits and industrial application value. Attached Figure Description

[0048] Figure 1 This is a system flowchart of the present invention;

[0049] Figure 2 This is a flowchart of the system technology of the present invention;

[0050] Figure 3 This is a schematic diagram of the initial state of the dynamic model of the six-axis robotic arm in this invention;

[0051] Figure 4 A flowchart for embedding sliding mode variable structure compensation terms in the admittance control loop;

[0052] Figure 5This is the execution flowchart for a multimodal coupled architecture.

[0053] Figure 6 This is a block diagram of the system structure of the present invention. Detailed Implementation

[0054] The present invention will now be further described with reference to the accompanying drawings and embodiments:

[0055] Example 1: Compliant interactive control process of ultrasonic robot based on multimodal dynamics compensation.

[0056] like Figure 1 , Figure 2 As shown, the present invention provides a compliant interactive control method for an ultrasonic robot, comprising the following steps:

[0057] Step S1: Build a hardware platform and construct a six-axis robotic arm dynamic model to calculate the nonlinear torque generated by the ultrasonic robot end effector during spatial motion in real time.

[0058] The system hardware platform employs a high-precision six-axis collaborative robotic arm, whose end flange connects to a high-frequency linear array ultrasonic probe (frequency range 5-15MHz) via a specially designed conversion interface. A six-dimensional force sensor is compactly integrated between the probe and the robotic arm's end effector. This sensor transmits the acquired three-dimensional force and torque signals to the central control workstation in real time via a high-speed Ethernet interface at a sampling frequency of 1000Hz. A vision sensor, mounted at the robotic arm's end effector, captures the geometric contours of the human body surface at a frame rate of no less than 30fps in real time. The central control workstation runs a real-time operating system (RTOS) to ensure the timing accuracy of each control loop.

[0059] like Figure 3 As shown, in the construction of the six-axis robotic arm dynamic model, the system first performs the establishment of the joint coordinate system and kinematic initialization. Using the robotic arm's reset posture as the global reference initial zero position, the six-axis robotic arm dynamic model is established based on the improved DH parameters. The Z-axis of each joint is set as the rotation axis, and the X-axis is defined on the common perpendicular of adjacent axes. By calculating the link lengths and offset distances between joints, a homogeneous transformation matrix chain from the base to the end effector is constructed. This homogeneous transformation matrix chain provides accurate geometric basis for subsequent gravity vector decomposition. The equations of the six-axis robotic arm dynamic model are:

[0060]

[0061] in, The joint space mass inertia matrix is ​​positive definite and symmetric. The matrix of Coriolis force and centrifugal force. This is the gravity term vector calculated from the positions of the centers of gravity of each link. This is a joint friction force matrix that includes both static and dynamic friction forces. For the driving torque vector, , , These represent joint angles, angular velocities, and angular accelerations, respectively. The six-axis robotic arm dynamics model considers the mass distribution of each link, the position of the center of mass, and the static and dynamic friction factors at the joints. In particular, it features a refined model for the ultrasonic probe and six-dimensional force sensor mounted at the end effector. Mass matrix. The Coriolis matrix is ​​obtained by superimposing the inertial parameters of each link through DH coordinate transformation. Calculation using Christoffel notation; gravity term Calculate the potential energy gradient of each link's center of mass under the current attitude; friction force. The LuGre friction model is used to accurately capture the nonlinear characteristics of the low-speed crawling phase.

[0062] Step S2: Based on the dynamic model of the six-axis robotic arm, perform dynamic gravity compensation on the end load consisting of the ultrasonic probe and the six-dimensional force sensor mounted on the end of the robotic arm, and extract pure end contact force data.

[0063] During ultrasonic scanning, the ultrasonic probe, six-dimensional force sensor, and various coupling components mounted on the end effector of the robotic arm have significant mass. As the scanning posture changes in three-dimensional space, the gravity vector of these end-effectors undergoes complex nonlinear changes, directly contaminating the raw signal fed back by the sensor. This step establishes a refined end-effector model to calculate and remove the gravity component from the sensor signal in real time.

[0064] As the robotic arm moves from its initial position toward the subject's scanning area, the real-time gravity compensation algorithm continuously executes: First, it reads the joint encoder data at the current moment. The real-time homogeneous transformation matrix of the end effector in the base coordinate system is calculated using the forward kinematic equations of the DH parameter method. Then the rotation matrix is ​​extracted. Then, based on the total mass pre-calibrated at the end load... and the position of the center of mass Calculate the gravity vector of the end load in the base coordinate system: Where g is the acceleration due to gravity. It is a vertically downward unit vector; next, from the sensor's raw data Remove gravity components and zero drift bias This yields a pure end contact force.

[0065] The corresponding dynamic gravity compensation formula is:

[0066] ;

[0067] in, To compensate for the pure end contact force, The raw force data collected by the six-dimensional force sensor. This is the real-time rotation matrix from the base coordinate system to the sensor coordinate system, which is calculated from the current joint encoder data. The gravity vector of the end load is represented in the base coordinate system. This is the zero-drift bias of the sensor, which is obtained through static calibration upon power-on.

[0068] During the continuous changes in scanning posture, the algorithm uses the pose information fed back by the joint encoder. The six-dimensional force sensor calculates and removes the gravity component from the sensor signal in real time at a sampling frequency of no less than 1000Hz, enabling the system to sense minute changes in contact force as low as 0.1N, thereby extracting pure end contact force data. This is the physical basis for achieving high-precision compliant control.

[0069] Step S3: Based on the amplitude of the obtained pure end contact force, during the interactive decision-making stage, adaptively switch between the zero-force guiding mode and the compliant follow-up mode according to the perceived pure force state, and smoothly interpolate the impedance parameters during the switching transition process.

[0070] To address the complex mode switching during ultrasound scanning, from large-scale spatial pathfinding to refined surface adhesion, this invention designs a multi-level adaptive impedance parameter adjustment mechanism, which includes two operating modes: zero-force guiding mode and compliant follow-up mode.

[0071] Preset safety threshold (Different values ​​are set in different embodiments, for example) ), and determine the pure end contact force. Has the safety threshold been exceeded? This confirms whether to use the zero-force guiding mode or the compliant follow mode. Details of mode switching and parameter adjustment are as follows:

[0072] (1) When the pure end contact force is below the safety threshold, In zero-force guided mode, the ultrasound robot minimizes virtual mass and virtual damping parameters. Active dynamic compensation counteracts the inertia of the robotic arm links, joint friction, and end-effector gravity, resulting in high transparency of the robotic arm. The ultrasound robot can then be guided by a minimal force, allowing for flexible movement of the robotic arm. Physicians only need to apply a minimal guiding force of no more than 5N to sensitively drag the robotic arm for scanning start-point positioning. Specifically, in zero-force guided mode, low-damping, high-transparency parameters are used: .

[0073] (2) Once the probe contacts the skin, the pure terminal contact force reaches the safety threshold. The ultrasound robot operates in compliant follow-up mode, increasing the virtual damping parameter to absorb the kinetic energy generated by the patient's breathing fluctuations. Low-stiffness admittance feedback ensures the probe maintains a constant adhesion force throughout the follow-up process, allowing for precise follow-up even if the patient experiences involuntary positional shifts, maintaining consistent acoustic coupling. Specifically, in compliant follow-up mode, increased damping stabilizes contact. ,in For adaptive gain.

[0074] (3) Using S-shaped logic functions for switching transitions ( The impedance parameters during the switching process are smoothly interpolated to avoid current jumps at the moment of switching. The expression for the smooth transition of the S-type logic function is as follows:

[0075] ;

[0076] in, To compensate for the pure end contact force, The preset safety threshold, For smooth interpolation of impedance parameters, It is an S-type logic function. These are the impedance parameter values ​​corresponding to the zero-force guided mode. The impedance parameter value corresponds to the compliant follow-up mode, avoiding current jumps and mechanical oscillations caused during switching. In the S-shaped logic function, the smooth switching weighting coefficient... for:

[0077] ;

[0078] in, The gain coefficient is used to control the steepness of the transition curve (k=10 in this embodiment). For the switching threshold (i.e.) ), The width of the transition interval (in this embodiment) ).when When the force approaches 0, the system is in zero-force guidance mode; when When the impedance approaches 1, the system is in compliant follower mode. All impedance parameters ( , , All according to The interpolation relationship of p is used to achieve a smooth transition in real time, realizing shockless mode switching.

[0079] Step S4: In compliant follower mode, the end displacement correction is generated by the second-order differential equation of admittance control; a sliding mode variable structure compensation term is embedded in the admittance control loop to cope with the nonlinear mechanical disturbances during the ultrasonic scanning process.

[0080] The second-order differential equation for admittance control is:

[0081] ;

[0082] in, To compensate for the pure end contact force, , , These are the virtual mass parameter matrix, virtual damping parameter matrix, and virtual stiffness parameter matrix, respectively. , , These are the desired position, velocity, and acceleration at the end point, respectively. , , These represent the position, velocity, and acceleration of the reference trajectory, respectively.

[0083] Traditional linear impedance models often suffer from phase lag and system oscillation risks when dealing with sudden nonlinear mechanical characteristics such as tissue thermal denaturation or instantaneous muscle spasms. For example... Figure 4 As shown, this invention addresses the force tracking accuracy problem in highly dynamic and nonlinear environments such as ablation surgery. Furthermore, this invention deeply embeds a sliding mode variable structure compensation algorithm into the admittance control loop. By constructing a state switching surface based on force error and its rate of change, the system's response bandwidth is dynamically adjusted. The process of embedding the sliding mode variable structure compensation term into the admittance control loop specifically includes:

[0084] (1) In order to achieve a rapid response to contact force error, this invention constructs a force tracking error model based on force error and its rate of change: ,in, To compensate for the pure end contact force, For target contact force;

[0085] (2) Construct a linear switching surface model based on force error and its rate of change: ,in For sliding surface parameters, (For example, (You can choose 10).

[0086] (3) To suppress high-frequency chattering in traditional sliding mode control, a continuous saturation function is adopted. The sign function is replaced to suppress high-frequency chattering, and the compensation control law is as follows: ,in The sliding mode gain is selected as 15 in this embodiment. The boundary layer thickness parameter is selected as 0.05 in this embodiment. The robustness gain is selected as 0.1 in this embodiment;

[0087] (4) The compensation control law is embedded as a disturbance compensation term into the second-order differential equation of the admittance control to form a modified admittance model. When the force deviation fluctuates drastically, the compensation term increases the control gain to force the error to converge toward the sliding surface; when the system tends to steady state, chattering is suppressed by a continuous switching function, and the tracking deviation of the time-varying force is limited to the millinewton range.

[0088] When a subject experiences sudden tremors or muscle spasms causing nonlinear force fluctuations, the compensation term rapidly forces error convergence by increasing the control gain, limiting the tracking deviation of the time-varying force to the millinewton range and effectively eliminating the lag phenomenon of traditional linear control in high-dynamic environments. Therefore, the mechanism of embedding a sliding mode variable structure compensation term in the admittance control loop in this invention not only significantly improves the system's speed in capturing time-varying force trajectories but also fundamentally solves the robustness problem of the admittance control system in complex interactive scenarios.

[0089] Step S5: In the underlying execution phase, a deep multimodal coupling architecture is established between image-based visual servoing technology and admittance correction commands. The system achieves high-precision autonomous scanning through this multimodal coupling architecture. The tangential path constraint commands generated by image-based visual servoing and the normal admittance force correction quantities generated in step S4 are decoupled and mapped in a unified joint space through an interaction matrix, driving the robotic arm to perform adaptive ultrasonic scanning.

[0090] like Figure 5 As shown, the multimodal coupling architecture includes a vision sensor and an admittance controller. The execution process of the multimodal coupling architecture includes:

[0091] (1) The visual sensor collects the geometric contour information (point cloud information) of the human body surface in real time at a frame rate of 30fps, and converts it into path constraints for tangential scanning through image processing.

[0092] (2) Normal displacement correction generated by the admittance controller Real-time intervention in the normal direction of movement step length;

[0093] (3) By real-time calculation of the interaction matrix L, the image feature error is mapped to the end velocity space, and then the normal admittance correction is superimposed and uniformly converted to the joint space for execution: ,in The pseudo-inverse of the Jacobian matrix of the robotic arm. It is the pseudo-inverse of the interaction matrix. This represents the rate of change of image feature error.

[0094] Through the above coupling mechanism, the robotic arm can maintain the probe axis at the optimal imaging angle based on visual feedback when performing scanning trajectories on complex anatomical surfaces such as the carotid aorta and the contours of abdominal organs. The contact force is maintained within the physiologically safe range of 2-6N through the admittance loop, thus achieving continuous acquisition of high-quality, standardized ultrasound images.

[0095] This invention decouples visual pose compensation and force-sensory compliant adhesion within a unified joint space. This coupling mechanism enables the robotic arm to perform high-precision trajectory planning along complex anatomical surfaces (such as the carotid artery or abdominal protrusions) in three-dimensional space, while simultaneously achieving adaptive adhesion to the skin surface in a "bio-tactile" manner. Through this integration of multi-dimensional dynamic compensation and active interaction logic, this invention not only solves the problem of rigid interaction in traditional ultrasound robots but also significantly improves the safety and reliability of the system in extreme clinical environments, providing solid technical support for the continuous acquisition of high-quality, standardized ultrasound images.

[0096] The compliant interactive control method for ultrasound robots based on multimodal dynamic compensation of this invention achieves significant leaps in performance compared to existing autonomous ultrasound scanning systems and industrial robotic arm control schemes, particularly in terms of inherent safety in human-machine interaction, flexibility in clinical operation, robustness of control in complex environments, and standardization of imaging quality. Through deep integration of the dynamic model, admittance control framework, and sliding mode compensation mechanism, the technical advantages of this invention are specifically reflected in the following dimensions:

[0097] (1) Achieved high-fidelity force perception accuracy based on dynamic solution.

[0098] The most fundamental and crucial technical effect of this invention lies in completely solving the problem of interference from the load at the end of the robotic arm on contact force sensing. During ultrasonic scanning, the ultrasonic probe, six-dimensional force sensor, and coupling components at various levels mounted on the end of the robotic arm have a significant mass.

[0099] As the scanning posture changes drastically in three-dimensional space, the gravity vectors of these loads undergo complex nonlinear changes, directly contaminating the raw signals fed back by the sensors. This invention, by establishing a refined six-axis robotic arm dynamics model and introducing a real-time gravity compensation algorithm, can calculate and eliminate the precise gravity components of the end effector in the current spatial pose with a millisecond-level response frequency. This approach allows the contact force deviation signal extracted by this invention to accurately and purely reflect the physical interaction between the probe and human tissue, eliminating the interference of gravity artifacts on the control system. This high-fidelity force perception capability is the logical starting point for subsequent compliant control, ensuring that the robot can perceive extremely minute changes in the stiffness of the subject's skin surface, thus providing a physical guarantee for maintaining the consistency of acoustic coupling.

[0100] (2) An inherently safe and compliant interaction boundary and clinical comfort were constructed.

[0101] Drawing inspiration from the interaction logic of rehabilitation robots designed to protect vulnerable limbs, this invention endows the ultrasound robot with tactile characteristics resembling a human hand through an admittance control algorithm.

[0102] Traditional position control schemes exhibit extremely high normal stiffness at the moment of contact, making them highly susceptible to instantaneous overshoot forces due to positioning errors, which can cause physiological and psychological impact on the subject. This invention, however, constructs a virtual buffer layer between the probe and the human body surface by adjusting impedance parameters such as virtual mass, virtual damping, and virtual stiffness. When the probe contacts a skin surface with uneven hardness distribution, the system can generate an instantaneous yielding displacement similar to a physical spring. This compliant interaction characteristic reduces the peak impact force at the moment of probe contact with the subject's skin by more than 80% compared to traditional control schemes, effectively suppressing medical safety risks caused by the rigidity of robot movement. Simultaneously, the dynamic compliance exhibited by this invention greatly alleviates the subject's sense of urgency and pressure during the examination, improving the operability of clinical examinations.

[0103] (3) It has broken through the bottleneck of physical load and operational flexibility in the physician teaching stage.

[0104] During the initial alignment phase of clinical scanning, physicians often need to manually guide the robot to a specific scanning plane. Due to the enormous physical inertia, joint static friction, and gravitational torque of traditional robotic arms, physicians must overcome significant resistance when manually dragging them.

[0105] This invention utilizes the "zero-force guidance" mode in admittance control, employing active dynamic compensation to enable the robotic arm to exhibit excellent "transparency" during teaching. The physician only needs to apply a minimal guiding force (typically less than 5N), and the system can sense the physician's movement intention and actively drive the robotic arm's joints to follow suit. This technology transforms strenuous physical labor into lightweight interactive guidance, not only lowering the operational threshold for ultrasound physicians but also significantly improving the efficiency of initial scan pose adjustment. Physicians can operate the multi-kilogram robotic end effector as easily as operating a lightweight handheld probe, achieving a perfect balance between operational flexibility and high payload capacity.

[0106] (4) Solved the problems of lag and oscillation in force tracking under dynamic environments such as ablation experiments.

[0107] During ablation procedures or high-dynamic scanning, the subject's respiratory movements, muscle tremors, and tissue thermal denaturation can cause drastic nonlinear shifts in the mechanical properties of the interactive environment. Traditional linear impedance control, when dealing with such sudden disturbances, often exhibits significant phase lag due to limited system bandwidth, leading to fluctuating probe-skin contact force and even resonance.

[0108] This invention significantly enhances the dynamic robustness of the control system by embedding a sliding mode variable structure compensation term in the admittance control loop. The sliding mode compensation term dynamically adjusts the system's response gain according to the changing trend of force error. When a micro-movement of the subject's limb or a sudden change in tissue impedance is detected, the compensation term forces the control error to converge rapidly towards the equilibrium plane. Experiments have shown that this mechanism effectively suppresses system oscillations and limits the tracking deviation of time-varying forces to the millinewton range. This highly robust technical effect ensures a constant signal-to-noise ratio in the ultrasound cross-sectional images throughout the examination cycle, avoiding anatomical deformation caused by pressure fluctuations and providing highly reliable original image support for accurate ablation assessment.

[0109] (5) Achieved standardized scanning quality with multimodal coupling of vision and force perception.

[0110] This invention achieves truly autonomous adaptive scanning by deeply coupling image-based visual servoing path commands with admittance compensation displacement commands in a multimodal manner. In complex anatomical section scanning, this invention can simultaneously meet the requirements of visual trajectory tracking accuracy and force-sensory compliance. This coupling mechanism spatially decouples and maps normal force adjustment commands and tangential path planning commands at the execution layer.

[0111] The technical advantages of this invention are as follows: when the robot scans along the dramatically curved surfaces of the human body (such as the carotid artery or groin area), the probe axis is consistently maintained at the optimal imaging angle based on visual feedback, while the contact force is kept within a preset physiological safety range through the admittance loop. This highly automated closed-loop control significantly reduces the uncertainty of human operation and achieves a high degree of standardization in the scanning process. Regardless of the subject's position, the system can acquire high-quality ultrasound images with clear anatomical details and consistent scale, significantly improving the repeatability and efficiency of clinical diagnosis, and possessing extremely high social benefits and industrial application value.

[0112] Example 2: Application of high dynamic force tracking in ablation surgery.

[0113] During radiofrequency ablation surgery, the physical properties (stiffness, viscoelasticity) of the treated tissue undergo drastic changes due to thermal denaturation, placing extremely high demands on the control of the contact force of the ultrasound probe. In this scenario, the system uses... As the target contact force, the sliding mode compensation gain Increased to 25, boundary layer thickness Reduced to 0.02 to improve the sensitivity of the compensation term to mechanical abrupt changes. When a sudden change in tissue impedance is detected ( The system automatically increases the sliding mode gain to 30, forcing the error to converge rapidly to the switching surface, and controlling the force fluctuation within ±5mN. Experimental results show that, compared with pure admittance control without embedded sliding mode compensation, the root mean square error of force tracking (RMSE) in ablation surgery scenarios is reduced by approximately 65%, effectively eliminating ultrasound image artifacts caused by force jumps and providing highly reliable original image support for ablation assessment.

[0114] Example 3: Software architecture implementation of a compliant interactive control system for an ultrasonic robot.

[0115] This invention also covers the system implementation of the above-described method at the software level. The software architecture is based on the ROS (Robot Operating System) framework and consists of the following core nodes:

[0116] (1) The dynamics_compensator node: subscribes to the joint encoder topic at a real-time frequency of 1000Hz, uses the dynamic model to solve the gravity compensation term online, and outputs the pure contact force. ;

[0117] (2) admission_controller node: subscribes at a frequency of 500Hz The topic is to execute the second-order differential equation of admittance control and the sliding mode compensation algorithm, and output the end displacement correction Δx;

[0118] (3) mode_switcher node: monitoring The impedance parameters are adjusted in real time according to the preset threshold and S-shaped logic function, and parameter update messages are sent to the admission_controller node.

[0119] (4) visual_servo node: Subscribes to visual sensor point cloud topics at a frequency of 30Hz, extracts image feature errors, solves the interaction matrix, and outputs tangential path constraint speed instructions;

[0120] (5) Coupling_executor node: It integrates admittance correction and visual servo instructions, generates joint velocity instructions through inverse kinematics / Jacobi mapping, and publishes them to the robotic arm driver for execution.

[0121] The aforementioned software architecture ensures the parallel operation and low-latency communication of each control module, and the overall control cycle meets real-time requirements, providing a reliable software foundation for the safe and stable operation of clinical ultrasound scanning.

[0122] Example 4: Compliant Interactive Control System for Ultrasonic Robots.

[0123] like Figure 6 As shown, the present invention also provides an ultrasonic robot compliant interactive control system for implementing the above-mentioned ultrasonic robot compliant interactive control method, which includes a hardware platform, a dynamic modeling and gravity compensation module, a multimodal adaptive impedance control module, an admittance control and sliding mode compensation module, and a visual servoing and multimodal coupling module.

[0124] The hardware platform includes a six-axis robotic arm, a high-frequency linear array ultrasonic probe, a six-dimensional force sensor, a vision sensor, and a central control workstation. The six-dimensional force sensor is integrated between the ultrasonic probe and the end flange of the six-axis robotic arm.

[0125] The dynamic modeling and gravity compensation module is used to establish the dynamic model of the six-axis robotic arm and calculate the gravity vector component of the end-effector load online based on the real-time feedback data from the joint encoder. It removes the influence of gravity from the raw sensor data and outputs the pure end-effector contact force.

[0126] The multimodal adaptive impedance control module is used to adaptively switch between zero-force guiding mode and compliant follow-up mode based on the amplitude of the pure end contact force, and to smoothly transition the impedance parameters.

[0127] The admittance control and sliding mode compensation module is used to execute the second-order differential equation of admittance control to calculate the end displacement correction, and to enhance the system's robustness to nonlinear mechanical disturbances by embedding sliding mode variable structure compensation terms.

[0128] The visual servoing and multimodal coupling module is used to decouple the tangential path constraints and admittance force corrections generated by visual servoing through an interaction matrix, and then execute them uniformly in the joint space to drive the robotic arm to complete adaptive ultrasound scanning.

[0129] Those skilled in the art will understand that various aspects of the present invention can be implemented as systems, methods, or program products. Therefore, various aspects of the present invention can be specifically implemented in the following forms: entirely hardware implementations, entirely software implementations (including firmware, microcode, etc.), or implementations combining hardware and software aspects, collectively referred to herein as “circuits,” “modules,” or “systems.”

[0130] Any one or more of the modules, submodules, units, and subunits according to this embodiment, or at least part of the functions of any one or more of them, can be implemented in one module. Any one or more of the modules, submodules, units, and subunits according to this embodiment can be implemented by dividing them into multiple modules. Any one or more of the modules, submodules, units, and subunits according to this embodiment can be at least partially implemented as hardware circuits, such as field-programmable gate arrays (FPGAs), programmable logic arrays (PLAs), systems-on-a-chip, systems-on-a-substrate, systems-on-package, application-specific integrated circuits (ASICs), or implemented by hardware or firmware in any other reasonable manner by integrating or packaging circuits, or implemented in software, hardware, and firmware, or in any appropriate combination of any of these three implementation methods. Alternatively, one or more of the modules, submodules, units, and subunits according to this embodiment can be at least partially implemented as computer program modules, which, when run, can perform corresponding functions.

[0131] Example 5: Computer-readable storage medium.

[0132] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the above-described ultrasonic robot compliant interactive control method.

[0133] Furthermore, although the operations of the method of the present invention are described in a specific order in the accompanying drawings, this does not require or imply that these operations must be performed in that specific order, or that all the operations shown must be performed to achieve the desired result. Additionally or alternatively, certain steps may be omitted, multiple steps may be combined into one step, and / or one step may be broken down into multiple steps.

[0134] It should be understood that those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims.

[0135] The present invention has been described above with reference to the accompanying drawings. Obviously, the implementation of the present invention is not limited to the above-described manner. Any improvements made using the inventive concept and technical solution of the present invention, or the direct application of the inventive concept and technical solution of the present invention to other situations without modification, are all within the protection scope of the present invention.

Claims

1. A compliant interactive control method for an ultrasonic robot, characterized in that, Includes the following steps: Step S1: Construct a six-axis robotic arm dynamic model to calculate the nonlinear torque generated by the end effector of the ultrasonic robot during spatial motion in real time; Step S2: Based on the dynamic model of the six-axis robotic arm, perform dynamic gravity compensation on the end load consisting of the ultrasonic probe and the six-dimensional force sensor mounted on the end of the robotic arm, and extract pure end contact force data. Step S3: Based on the amplitude of the obtained pure end contact force, adaptively switch between zero-force guiding mode and compliant follow-up mode, and smoothly interpolate the impedance parameters during the switching transition process. Step S4: In compliant follower mode, the end displacement correction is generated by the second-order differential equation of admittance control; a sliding mode variable structure compensation term is embedded in the admittance control loop to cope with the nonlinear mechanical disturbances during the ultrasonic scanning process. Step S5: Establish a multimodal coupling architecture. Decouple the tangential path constraint command generated by image-based visual servoing and the normal directional admittance correction amount generated in step S4 through an interaction matrix in a unified joint space to drive the robotic arm to perform adaptive ultrasound scanning.

2. The compliant interactive control method for an ultrasonic robot according to claim 1, characterized in that, In step S1, the robot arm reset posture is used as the global reference initial zero position, the Z-axis of each joint is set as the rotation axis, the X-axis is defined on the common perpendicular line of adjacent axes, and the homogeneous transformation matrix chain from the base to the end probe is constructed by calculating the link length and offset distance between the joints.

3. The compliant interactive control method for an ultrasonic robot according to claim 2, characterized in that, A dynamic model of a six-axis robotic arm is established based on the improved DH parameters. The equations of the six-axis robotic arm dynamic model are as follows: ； in, Here is the joint space mass inertia matrix. The matrix of Coriolis force and centrifugal force. The vector of the gravity term. Here is the joint friction force matrix. For the driving torque vector, , , These are joint angle, angular velocity, and angular acceleration, respectively.

4. The compliant interactive control method for an ultrasonic robot according to claim 1, characterized in that, In step S2, the dynamic gravity compensation formula is: ； in, To compensate for the pure end contact force, The raw force data collected by the six-dimensional force sensor. This is the real-time rotation matrix from the base coordinate system to the sensor coordinate system. The gravity vector of the end load. This is the zero-drift bias of the sensor.

5. The compliant interactive control method for an ultrasonic robot according to claim 1, characterized in that, In step S3, a safety threshold is preset, and it is determined whether the pure end contact force exceeds the safety threshold: when the pure end contact force is lower than the safety threshold, the ultrasonic robot is in zero-force guidance mode, the virtual mass and virtual damping parameters are adjusted to the minimum value, and the robotic arm is made to have high transparency through active dynamic compensation. The ultrasonic robot can be driven to move flexibly by applying guidance force. When the pure end contact force reaches the safety threshold, the ultrasonic robot enters a compliant follow-up mode and increases the virtual damping parameter to absorb the kinetic energy generated by the human body's breathing fluctuations.

6. The compliant interactive control method for an ultrasonic robot according to claim 1, characterized in that, In step S3, the impedance parameters during the switching transition are smoothly interpolated using an S-type logic function. The expression for the parameter smoothing transition of the S-type logic function is as follows: ； in, For smooth interpolation of impedance parameters, It is an S-type logic function. These are the impedance parameter values ​​corresponding to the zero-force guided mode. These are the impedance parameter values ​​corresponding to the compliant follower mode. To compensate for the pure end contact force, This is a preset safety threshold.

7. The compliant interactive control method for an ultrasonic robot according to claim 1, characterized in that, In step S4, the admittance control second-order differential equation is: ； in, To compensate for the pure end contact force, , , These are the virtual mass parameter matrix, virtual damping parameter matrix, and virtual stiffness parameter matrix, respectively. , , These are the desired position, velocity, and acceleration at the end point, respectively. , , These represent the position, velocity, and acceleration of the reference trajectory, respectively.

8. The compliant interactive control method for an ultrasonic robot according to claim 1, characterized in that, In step S4, the process of embedding a sliding mode variable structure compensation term in the admittance control loop specifically includes: (1) Constructing the force tracking error model: ,in, To compensate for the pure end contact force, For target contact force; (2) Construct a linear switching surface model based on force error and its rate of change: , where λ is the sliding surface parameter; (3) Use a continuous saturation function The sign function is replaced to suppress high-frequency chattering, and the compensation control law is as follows: ,in For sliding mode gain, For boundary layer thickness parameters, For robustness term gain; (4) The compensation control law is embedded as a disturbance compensation term into the second-order differential equation of the admittance control to form the modified admittance model.

9. A compliant interactive control system for an ultrasonic robot, characterized in that, The ultrasonic robot compliant interactive control method for implementing any one of claims 1 to 8 comprises: The hardware platform includes a six-axis robotic arm, a high-frequency linear array ultrasonic probe, a six-dimensional force sensor, a vision sensor, and a central control workstation. The six-dimensional force sensor is integrated between the ultrasonic probe and the end flange of the six-axis robotic arm. The dynamic modeling and gravity compensation module is used to establish a dynamic model of a six-axis robotic arm and calculate the gravity vector component of the end-effector load online based on real-time feedback data from the joint encoder. It removes the influence of gravity from the raw sensor data and outputs a pure end-effector contact force. The multi-modal adaptive impedance control module is used to adaptively switch between zero-force guiding mode and compliant follow-up mode based on the amplitude of the pure end contact force, and to smoothly transition the impedance parameters. The admittance control and sliding mode compensation module is used to execute the second-order differential equation of admittance control to calculate the end displacement correction, and to enhance the system's robustness to nonlinear mechanical disturbances by embedding sliding mode variable structure compensation terms. The visual servoing and multimodal coupling module is used to decouple the tangential path constraints and admittance force corrections generated by visual servoing through an interaction matrix, and then execute them uniformly in the joint space to drive the robotic arm to complete adaptive ultrasound scanning.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the ultrasonic robot compliant interactive control method according to any one of claims 1 to 8.

Images