Ultrasonic positioning method and system based on visual servo and storage medium
By using a multimodal feature fusion pose estimation model and contact force feedback, the positioning accuracy and efficiency issues of ultrasound probes in medical robots in soft tissue environments were solved, achieving high-precision ultrasound probe control.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- WUXI AMIT CO LTD
- Filing Date
- 2025-12-23
- Publication Date
- 2026-05-12
AI Technical Summary
Traditional position-based visual servoing in medical robots suffers from low positioning accuracy and efficiency of ultrasound probes due to the lack of stable features on the surface of human soft tissue.
A multimodal feature fusion pose estimation model combined with contact force feedback is adopted. RGB and depth image features are extracted through a pre-trained two-stream neural network and PointNet++ structure. Impedance compensation and pose correction errors are calculated to generate precise control commands.
It improves the positioning accuracy and adaptability of ultrasound probes in soft tissue environments, realizes closed-loop control of the entire process from perception to decision-making to execution, and enhances the stability and efficiency of positioning.
Smart Images
Figure CN122004931A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of medical control, and specifically to a visual servo-based ultrasound positioning method, system, and storage medium. Background Technology
[0002] Position-Based Visual Servoing (PBVS) is a technology that controls robot movement through visual information. Its core is to convert image features into three-dimensional spatial information, thereby controlling the robot's end effector to achieve the target pose.
[0003] The inventors discovered that traditional position-based visual servoing in medical robot applications typically relies on 3D reconstruction of image feature points for localization. However, the surface of human soft tissue lacks stable physical features such as skeletal edges and rigid body edges. The skin, muscles, and other tissues in the contact area of the ultrasound probe are prone to deformation under stress, causing feature points to drift and thus affecting positioning accuracy.
[0004] The above problems urgently need to be solved. Summary of the Invention
[0005] In view of this, the present invention provides a visual servo-based ultrasonic positioning method, system and storage medium to solve the technical problems of low positioning accuracy and low efficiency of ultrasonic probes in the prior art.
[0006] In a first aspect, the present invention provides a visual servoing-based ultrasonic positioning method, comprising: Acquire the RGB image and depth image at the current moment, the contact force data at the current moment, and the current actual pose of the ultrasonic probe; Based on the RGB image and depth image at the current moment, the current target pose of the ultrasound probe is obtained through a pre-trained multimodal feature fusion pose estimation model. Based on the pose error between the current actual pose and the current target pose, a basic pose correction error for the ultrasonic probe is generated. Based on the contact force data at the current moment and the preset reference contact force threshold, impedance compensation is calculated, and combined with the basic pose correction error, a target correction command is generated to control the ultrasonic probe to reach the target pose.
[0007] As an optional implementation, the multimodal feature fusion pose estimation model adopts a two-stream neural network structure; The RGB branch employs a ResNet-18 structure with the fully connected layers removed, including convolutional layers, pooling layers, and four residual blocks, outputting a 512-dimensional feature vector. . The deep branch uses the PointNet++ architecture, including sampling layers, grouping layers, and multi-layer perceptron layers, outputting a 512-dimensional feature vector. ; Feature fusion layer, which fuses the feature vectors and the feature vector The formula is: in, , These are learnable parameters; fused feature vector After pose regression processing by the pose regression layer, the position translation vectors are output respectively. With attitude rotation quaternion vector and the estimated confidence level ; The loss function for the pose regression process is: in, For the actual location, For the true attitude, , , These are the weighting coefficients.
[0008] As an optional implementation, the formula for generating the basic pose correction error of the ultrasonic probe is: in, For the target location, For the target posture, Current position As for the current stance, This indicates taking the imaginary part of the quaternion.
[0009] As an optional implementation, the formula for calculating impedance compensation is: in, The preset reference contact force threshold, This is the Z-axis measurement of the contact force at the current moment. This is the difference in contact force. For force gain coefficient, , For impedance compensation speed.
[0010] As an optional implementation, the target correction command is generated by combining the basic pose correction error, and the formula is: in, For the pseudo-inverse of the Jacobian matrix of the robotic arm, It is a proportional gain matrix. To control the velocity vector.
[0011] As an optional implementation, the visual servoing-based ultrasonic positioning method further includes: The system detects whether the current contact force exceeds a preset safety threshold. If it exceeds or approaches the preset safety threshold, a gradient backoff mechanism is activated, linearly decreasing the control speed according to the contact force difference. The formula is as follows: in, This is the threshold for initiating the gradient backoff mechanism; The system monitors whether the linear velocity and angular velocity in the control velocity vector exceed preset thresholds, limits the velocity components that exceed these thresholds, and simultaneously monitors acceleration changes.
[0012] As an optional implementation, the visual servoing-based ultrasonic positioning method further includes: If the estimated confidence level is lower than a preset threshold, the target pose from the previous time step is retained.
[0013] Secondly, the present invention provides a visual servoing-based ultrasonic positioning system, which employs the visual servoing-based ultrasonic positioning method provided in the first aspect above, including: The acquisition module is used to acquire the RGB image and depth image at the current moment, the contact force data at the current moment, and the current actual pose of the ultrasonic probe; The estimation module is used to obtain the current target pose of the ultrasound probe based on the RGB image and depth image at the current time, through a pre-trained multimodal feature fusion pose estimation model. The basic module is used to generate the basic pose correction error of the ultrasonic probe based on the pose error between the current actual pose and the current target pose. The compensation module is used to calculate impedance compensation based on the contact force data at the current moment and the preset reference contact force threshold, and to generate a target correction command in combination with the basic pose correction error, so as to control the ultrasonic probe to reach the target pose.
[0014] Thirdly, the present invention provides a computer device, comprising: a memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the computer instructions to implement the visual servo-based ultrasonic positioning method described in the first aspect.
[0015] Fourthly, the present invention provides a computer-readable storage medium storing computer instructions that, when executed by a processor, implement the visual servo-based ultrasonic positioning method described in the first aspect.
[0016] The technical solution provided by this invention first acquires the current visual data, force sensor data, and the actual pose of the probe; then, it estimates the target pose using a pre-trained multimodal model; next, it generates a basic correction amount based on the error between the actual and target poses; finally, it combines force feedback for impedance compensation to generate the final control command, achieving precise positioning. Multimodal information fusion, combining visual and force information, improves positioning accuracy and adaptability in dynamic soft tissue environments. It achieves closed-loop control throughout the entire process from perception to decision-making to execution, applicable to various ultrasonic robots and working scenarios, improving the applicability and efficiency of ultrasonic probe positioning. Attached Figure Description
[0017] To more clearly illustrate and understand the technical solutions in the embodiments of the present invention, the accompanying drawings used in the background technology and embodiment descriptions of the present invention will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the content of the embodiments of the present invention and these drawings without creative effort.
[0018] Figure 1 A schematic diagram of a visual servoing-based ultrasonic positioning method is shown. Figure 2 A schematic diagram of a multimodal feature fusion pose estimation model is shown. Detailed Implementation
[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0020] Example 1 This embodiment provides a visual servoing-based ultrasound positioning method applied to a medical ultrasound examination robot. The robot includes: a robotic arm, an ultrasound probe mounted at the end of the robotic arm, a force sensor mounted between the end of the robotic arm and the ultrasound probe, and an RGB-D camera for acquiring RGB and depth images. The force sensor is used to acquire the contact force between the ultrasound probe and the contact area. The RGB-D camera can be mounted on the robotic arm with the image acquisition direction consistent with the ultrasound probe's acquisition direction, or it can be mounted in a fixed position above the ultrasound examination stage.
[0021] See Figure 1 As shown, the present invention provides an ultrasonic positioning method based on visual servoing, which includes the following steps.
[0022] S1. Acquire the RGB image and depth image at the current moment, the contact force data at the current moment, and the current actual pose of the ultrasonic probe; S2. Based on the RGB image and depth image at the current moment, the current target pose of the ultrasound probe is obtained through a pre-trained multimodal feature fusion pose estimation model. S3. Based on the pose error between the current actual pose and the current target pose, generate the basic pose correction error of the ultrasonic probe. S4. Based on the current contact force data and the preset reference contact force threshold, calculate the impedance compensation, and combine it with the basic pose correction error to generate a target correction command to control the ultrasonic probe to reach the target pose.
[0023] The current actual pose of the ultrasonic probe is determined by the coding feedback of the robotic arm, thereby determining the precise pose of the robotic arm's end effector, which in turn determines the current actual position and orientation of the ultrasonic probe.
[0024] This invention first acquires visual data, force sensor data, and the actual pose of the probe at the current moment; then, it estimates the target pose using a pre-trained multimodal model; next, it generates a basic correction value based on the error between the actual and target poses; finally, it combines force feedback for impedance compensation to generate the final control command, achieving precise positioning. By integrating RGB and depth image information into a multimodal feature fusion pose estimation model, and simultaneously introducing impedance compensation through contact force feedback, it effectively offsets positioning deviations caused by soft tissue deformation, significantly improving the positioning accuracy and adaptability of the ultrasonic probe in dynamic soft tissue environments. It achieves closed-loop control throughout the entire process from perception to decision-making to execution, applicable to various ultrasonic robots and working scenarios, improving the applicability and efficiency of ultrasonic probe positioning.
[0025] Furthermore, pixel value normalization preprocessing is performed on the RGB image, and the depth image is preprocessed by converting the depth image into a point cloud and removing background points with a distance greater than a preset threshold.
[0026] See Figure 2 As shown, the multimodal feature fusion pose estimation model adopts a two-stream neural network structure; The RGB branch employs a ResNet-18 structure with the fully connected layers removed, including convolutional layers, pooling layers, and four residual blocks, outputting a 512-dimensional feature vector. . The deep branch uses the PointNet++ architecture, including sampling layers, grouping layers, and multi-layer perceptron layers, outputting a 512-dimensional feature vector. ; Feature fusion layer, which fuses the feature vectors and the feature vector The formula is: in, , These are learnable parameters; fused feature vector After pose regression processing by the pose regression layer, the position translation vectors are output respectively. With attitude rotation quaternion vector and the estimated confidence level .
[0027] If the confidence level is lower than a preset threshold, the target pose from the previous moment is retained. This avoids the impact of unreliable pose estimation results on positioning accuracy and motion stability.
[0028] The loss function for pose regression is: in, For the actual location, For the true attitude, , , These are the weighting coefficients. The loss function combines position error and attitude error.
[0029] A dual-stream neural network architecture is employed to adapt to the feature extraction requirements of visual images and point cloud data respectively. The feature vectors output by the two branches have the same dimension. The feature fusion layer fuses the features from the two branches through a formula with learnable parameters. After fusion, the pose regression layer outputs a position translation vector, a pose rotation quaternion vector, and an estimated confidence score. The regression process uses a loss function with weighted coefficients for optimization to ensure the accuracy of pose estimation. ResNet-18 and PointNet++ are used to extract features from images and point clouds respectively, improving feature representation ability and pose estimation accuracy. The fusion of multimodal features can compensate for the shortcomings of a single sensor in specific environments. Adaptive feature fusion is achieved through learnable weight parameters, improving the model's adaptability to different scenarios.
[0030] The formula for generating the basic pose correction error of the ultrasonic probe is: in, For the target location, For the target posture, Current position As for the current stance, This indicates taking the imaginary part of the quaternion.
[0031] By calculating the position difference and the imaginary part difference of the attitude quaternion, the deviation between the current pose and the target pose of the ultrasonic probe is quantified, providing a core basis for pose correction in the subsequent generation of control commands. The imaginary part of the quaternion is used to accurately characterize the attitude rotation deviation. Using quaternions to represent the attitude error avoids Euler angle singularity problems and improves computational stability. Position and attitude errors are calculated separately, facilitating independent adjustment and optimization of the controller. This method is suitable for high-frequency calculations in real-time control.
[0032] The formula for calculating impedance compensation is: in, The preset reference contact force threshold, This is the Z-axis measurement of the contact force at the current moment. This is the difference in contact force. For force gain coefficient, , For impedance compensation speed.
[0033] Based on the difference between a preset reference contact force threshold and the current Z-axis measurement of the contact force, and combined with a force gain coefficient, the impedance compensation speed is calculated. This compensation amount is used to address positioning deviations caused by the deformation of human soft tissue under stress, while also adapting to dynamic changes in contact force. The probe movement is dynamically adjusted according to the real-time contact force, enhancing compliance with soft tissue. The impedance compensation mechanism prevents excessive pressure from the probe on the tissue, improving operational safety. Stable contact force contributes to obtaining clearer and more stable ultrasound images.
[0034] Based on the basic pose correction error, the formula for generating the target correction command is as follows: in, For the pseudo-inverse of the Jacobian matrix of the robotic arm, It is a proportional gain matrix. To control the velocity vector.
[0035] By integrating the pseudo-inverse of the robotic arm's Jacobian matrix, the proportional gain matrix, and the control velocity vector, matrix operations are performed to combine basic pose correction error and impedance compensation, generating the final control command. This achieves an organic combination of pose correction and force feedback compensation, ensuring the ultrasound probe moves along a precise path. The control law is improved, achieving accurate mapping from task space to joint space based on the Jacobian matrix, thus enhancing control precision. The proportional gain matrix can adjust the system response speed to adapt to different motion scenarios. The integration of visual and force commands improves the overall stability and robustness of the positioning.
[0036] Before executing the target correction command, it checks whether the current contact force exceeds a preset safety threshold. If it exceeds or is close to the preset safety threshold, a gradient backoff mechanism is activated, linearly decreasing the control speed according to the contact force difference. The formula is as follows: in, This is the threshold for initiating the gradient backoff mechanism.
[0037] If the contact force at the current moment significantly exceeds the preset safety threshold, the robotic arm will immediately stop moving and the preset safety protocol will be triggered.
[0038] The system monitors whether the linear velocity and angular velocity in the control velocity vector exceed preset thresholds, limits the amplitude of any exceeding velocity components, and simultaneously monitors acceleration changes. This ensures smooth, shock-free movement of the robotic arm and avoids sudden stops and starts.
[0039] A safety control mechanism has been introduced, including gradient backoff and speed limiting. When the contact force approaches or exceeds the safety threshold, the system automatically reduces the controlled speed; simultaneously, it monitors the motion speed and acceleration to ensure smooth movement. Combining force feedback and motion monitoring, a multi-layered safety protection system is constructed. Speed limiting and acceleration monitoring prevent sudden stops and starts of the robotic arm, improving motion smoothness. The gradient backoff mechanism automatically intervenes in case of abnormal force to prevent damage to equipment or tissue.
[0040] Example 2 This embodiment provides a visual servoing-based ultrasonic positioning system, which is applied to the visual servoing-based ultrasonic positioning method provided in Embodiment 1 above for illustration. The visual servoing-based ultrasonic positioning system provided by this invention may include the following multiple modules.
[0041] The acquisition module is used to acquire the RGB image and depth image at the current moment, the contact force data at the current moment, and the current actual pose of the ultrasonic probe; The estimation module is used to obtain the current target pose of the ultrasound probe based on the RGB image and depth image at the current time, through a pre-trained multimodal feature fusion pose estimation model. The basic module is used to generate the basic pose correction error of the ultrasonic probe based on the pose error between the current actual pose and the current target pose. The compensation module is used to calculate impedance compensation based on the current contact force data and the preset reference contact force threshold, and to generate target correction commands in combination with the basic pose correction error to control the ultrasonic probe to reach the target pose.
[0042] The visual servo-based ultrasonic positioning system provided in this embodiment of the invention can be applied to the visual servo-based ultrasonic positioning method provided in Embodiment 1 above. For relevant details, please refer to the above method embodiment. Its implementation principle and technical effect are similar, and will not be repeated here.
[0043] It should be noted that the visual servoing-based ultrasonic positioning system provided in this embodiment of the invention is only illustrated by the above-described division of functional modules / units. In practical applications, the functions described above can be assigned to different functional modules / units as needed, that is, the internal structure of the visual servoing-based ultrasonic positioning system can be divided into different functional modules / units to complete all or part of the functions described above. Furthermore, the implementation method of the visual servoing-based ultrasonic positioning method provided in Embodiment 1 above and the implementation method of the visual servoing-based ultrasonic positioning system provided in Embodiment 2 belong to the same concept. The specific implementation process of the visual servoing-based ultrasonic positioning system provided in Embodiment 2 is detailed in Embodiment 1 above, and will not be repeated here.
[0044] Example 3 This embodiment provides a computer device, including but not limited to a processor and a memory. The memory stores computer instructions, and the processor executes the computer instructions to implement the method described above.
[0045] One embodiment of the present invention also provides a computer-readable storage medium for storing a computer program, which, when executed by a processor, implements the method described in the above-described method embodiments.
Claims
1. A visual servoing-based ultrasonic positioning method, characterized in that, include: Acquire the RGB image and depth image at the current moment, the contact force data at the current moment, and the current actual pose of the ultrasonic probe; Based on the RGB image and depth image at the current moment, the current target pose of the ultrasound probe is obtained through a pre-trained multimodal feature fusion pose estimation model. Based on the pose error between the current actual pose and the current target pose, a basic pose correction error for the ultrasonic probe is generated. Based on the contact force data at the current moment and the preset reference contact force threshold, impedance compensation is calculated, and combined with the basic pose correction error, a target correction command is generated to control the ultrasonic probe to reach the target pose.
2. The ultrasonic positioning method based on visual servoing according to claim 1, characterized in that, The multimodal feature fusion pose estimation model adopts a two-stream neural network structure; The RGB branch employs a ResNet-18 structure with the fully connected layers removed, including convolutional layers, pooling layers, and four residual blocks, outputting a 512-dimensional feature vector. . The deep branch uses the PointNet++ architecture, including sampling layers, grouping layers, and multi-layer perceptron layers, outputting a 512-dimensional feature vector. ; Feature fusion layer, which fuses the feature vectors and the feature vector The formula is: in, , These are learnable parameters; fused feature vector After pose regression processing by the pose regression layer, the position translation vectors are output respectively. With attitude rotation quaternion vector and the estimated confidence level ; The loss function for the pose regression process is: in, For the actual location, For the true attitude, , , These are the weighting coefficients.
3. The ultrasound positioning method based on visual servoing according to claim 2, characterized in that, The formula for correcting the basic pose of the generated ultrasonic probe is: in, For the target location, For the target posture, Current position As for the current stance, This indicates taking the imaginary part of the quaternion. As an optional implementation, the formula for calculating impedance compensation is: in, The preset reference contact force threshold, This is the Z-axis measurement of the contact force at the current moment. This is the difference in contact force. For force gain coefficient, , For impedance compensation speed.
4. The ultrasound positioning method based on visual servoing according to claim 3, characterized in that, The formula for calculating impedance compensation is as follows: in, The preset reference contact force threshold, This is the Z-axis measurement of the contact force at the current moment. This is the difference in contact force. For force gain coefficient, , For impedance compensation speed.
5. The ultrasonic positioning method based on visual servoing according to claim 4, characterized in that, The target correction command is generated by combining the aforementioned basic pose correction error, using the following formula: in, For the pseudo-inverse of the Jacobian matrix of the robotic arm, It is a proportional gain matrix. To control the velocity vector.
6. The ultrasonic positioning method based on visual servoing according to claim 5, characterized in that, Also includes: The visual servo-based ultrasonic positioning method further includes: The system detects whether the current contact force exceeds a preset safety threshold. If it exceeds or approaches the preset safety threshold, a gradient backoff mechanism is activated, linearly decreasing the control speed according to the contact force difference. The formula is as follows: in, This is the threshold for initiating the gradient backoff mechanism; The system monitors whether the linear velocity and angular velocity in the control velocity vector exceed preset thresholds, limits the velocity components that exceed these thresholds, and simultaneously monitors acceleration changes.
7. The ultrasonic positioning method based on visual servoing according to claim 6, characterized in that, Also includes: If the estimated confidence level is lower than a preset threshold, the target pose from the previous time step is retained.
8. A visual servo-based ultrasonic positioning system, characterized in that, include: The acquisition module is used to acquire the RGB image and depth image at the current moment, the contact force data at the current moment, and the current actual pose of the ultrasonic probe; The estimation module is used to obtain the current target pose of the ultrasound probe based on the RGB image and depth image at the current time, through a pre-trained multimodal feature fusion pose estimation model. The basic module is used to generate the basic pose correction error of the ultrasonic probe based on the pose error between the current actual pose and the current target pose. The compensation module is used to calculate impedance compensation based on the contact force data at the current moment and the preset reference contact force threshold, and to generate a target correction command in combination with the basic pose correction error, so as to control the ultrasonic probe to reach the target pose.
9. A computer device, characterized in that, include: The system includes a memory and a processor, which are interconnected and the memory stores computer instructions. The processor executes the computer instructions to implement the visual servo-based ultrasonic positioning method as described in any one of claims 1-7.
10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions that, when executed by a processor, implement the visual servo-based ultrasonic positioning method as described in any one of claims 1-7.