CN115670505A - 一种基于多模态融合的超声扫描控制系统 - Google Patents

Abstract

本发明公开了一种基于多模态融合的超声扫描控制系统。所述系统包括相机、超声探头、机器人、力传感器和上位机；其中，力传感器和超声探头均固定于机器人末端，相机、力传感器和超声探头均连接上位机并向其传输待检测目标的色彩深度信息、接触力信息和超声图像；上位机根据接收的信息向机器人发送控制指令；机器人连接上位机并根据上位机发送的指令进行运动并反馈自身运动状态。本发明在上位机中构建的基于自适应阻抗控制策略在探头法向上建立自适应力控制器，该控制器可以根据环境信息的变化自适应调整阻抗模型的阻尼系数，提高在复杂环境中的抗干扰能力。

Description

一种基于多模态融合的超声扫描控制系统

技术领域

本发明涉及智能机器人技术领域，尤其是涉及一种基于多模态融合的超声扫描控制系统。

背景技术

作为现代四大医学成像技术之一，超声扫描技术是一项人类疾病检查和诊断中有效的医疗措施，在日常生活中有着广泛的应用。超声成像技术通过发出高频超声波与反射波构建人体内部组织和器官的影像。在日常的超声检查中，超声医师需要手动调节超声探头采集超声影像。影像的质量依赖于超声医师对于人体组织结构的理解。在超声检查成像过程中，超声医师需要固定姿势保持长时间的接触条件，这一系列超负荷工作容易引发职业病。由于超声医师直接与患者接触，暴露在病原体下，对于工作在感染科的一线超声医师来说，这存在着巨大的被感染风险。

随着机器人行业的发展，现代机器人已经具备应对多变环境的能力，将机器人引入超声扫描控制领域，可以避免医务人员与患者之间的直接接触，采集到标准化的超声成像。目前，特殊用途的超声扫描机器人已经在工业领域开始了商业化，如自动乳房超声(ABUS)和机器人经颅多普勒(TCD)系统。TCD系统已经成功用于检测COVID-19新冠患者的肺血管扩张^[1][2][3]。

在远程超声扫描系统中，超声医师可以通过操纵杆远程实时操纵检查现场的机器人。目前，Sotiris等人提出了一种基于便携式机器人的移动超声远程检查平台，可以实现端到端的心脏远程超声检查。在5G通信网络的快速发展下，多个远程系统应用在COVID-19爆发器件的远程肺、心脏和脉管系统的检查与诊断，但是在这一套系统中所研究的探头的运动轨迹仍然由超声医生指令操作，并未改善操作者工作量大的现象，超声图像的成像水平取决于人为因素(Wu S,Wu D,Ye R,et al.Pilot Study of Robot-AssistedTeleultrasound Based on 5G Network:A New Feasible Strategy for Early ImagingAssessment during COVID-19 Pandemic[J].IEEE transactions on ultrasonics,ferroelectrics,and frequency control,2020,PP(11).)。

由于引进了自动化技术，自动化超声扫描系统不完全依赖于医学专家的远程控制，机器人可以承担更多的扫描任务，Tao等人提出的一种新型超声扫描机器人控制结构，实现了切向位移控制与法向模糊自适应阻抗控制，可以得到清晰的超声图像。Nakahara等人提出的超声悬浮装置，可以通过对声场的控制弥补机器人末端机械手的定位不确定性，实现控制施加在目标物体的抓取力，机器人从而获得操控毫米级对象的能力。Selcudean等人开发的用于检查颈动脉的超声扫查系统，通过图像特征跟踪算法实现了视觉伺服控制。

在日常的超声检查中，为了实现高质量超声图像的采集，需要在探头与人体组织间保持一定的接触条件，此过程需要使用力位控制算法，常见的力位控制算法包含力位混合控制与阻抗控制技术。力位混合控制算法利用对角矩阵在不同的子空间中分别处理力和位置运动信息，需要机器人在其末端工具坐标系下清晰地划分成力任务和位置任务。阻抗控制可以在一个方向对力和位置同时控制，适用于需要在某个特定方向上进行对位置精度和跟踪力做出折中的情况。

对于医学影响的评估，一种方式是通过人的主观判断，另一种方式是通过数学计算的方式，其中包括全参考、半参考与无参考三种评估方法。通过数学计算的方式中的前两种评估方法基于标准参考图像，并借助各种质量评估标准将标准参考图像与测试图像进行对比，在实际应用中，难以获得标准原始图像作为参考，所以无参考图像质量评估得到了广泛的关注。部分研究者提出通过提取图像中的关键特征构建图像质量评估标准。在超声扫描的图像中通常存在噪点，图像中的组织不清晰，这个问题造成了超声图像自动评估的复杂性。因此，构建的超声图像质量评估算法应能区分图像中的不同特征。

发明内容

本发明提供一种基于多模态融合的超声扫描控制系统，将机器人引入超声扫描控制领域，采集到更标准化的超声图像，提高检查的可重复性，在面对复杂接触环境和轨迹时，在线估计合适的跟踪力进行超声扫描，对超声图像质量进行评估。

本发明的目的至少通过如下技术方案之一实现。

一种基于多模态融合的超声扫描控制系统，包括相机、超声探头、机器人、力传感器和上位机；

其中，力传感器和超声探头均固定于机器人末端，相机、力传感器和超声探头均连接上位机并向其传输待检测目标的色彩深度信息、接触力信息和超声图像；上位机根据接收的信息向机器人发送控制指令；机器人连接上位机并根据上位机发送的指令进行运动并反馈自身运动状态。

进一步地，力传感器和超声探头通过夹具固定于机器人末端，其中，超声探头刚性固定于机器人末端；力传感器一端连接夹具上端，另一端连接机器人末端，获取超声探头和待检测目标的接触力大小。

进一步地，上位机中构建有坐标系转换模块；

机器人初始化阶段，坐标系转换模块通过手眼标定算法建立相机坐标系和机器人基坐标系之间的位姿变换关系，将上位机中提前规划好的超声扫描点空间坐标从相机坐标系下变换到机器人基坐标系。

进一步地，上位机中构建有力传感器重力补偿模块；

由于夹具和超声探头本身的重力，在进行力控制之前需要对其进行补偿，力传感器重力补偿模块在系统初始化前需要计算超声探头及夹具的重力以及力传感器的零点偏移值，对力传感器测量得到的力值数据进行补偿，最终计算得到真实的接触力；

六维力传感器所测量的力在六维力传感器坐标轴x,y,z轴上的分量为(F_x,F_y,F_z)，在六维力传感器坐标轴x,y,z轴上的力矩分量为(M_x,M_y,M_z)，设定六维力传感器的测量值为F＝[F_x,F_y,F_z,M_x,M_y,M_z]；零点值为

由探头和夹具组成的末端工具在该六维力传感器坐标系下的力和力矩为

末端工具重力G在六维力传感器坐标轴上的力矩分量

由下列公式可得：

其中(x_o,y_o,z_o)是末端工具质心在六维力传感器坐标系中的空间坐标；在静态标定时无外部作用力作用在末端工具上，

i＝x,y,z；因此，末端工具与环境的真实接触力

的公式为：

进一步地，上位机中构建有轨迹控制器模块；

轨迹控制器模块接收上位机发送的超声扫描点空间坐标，将接收的超声扫描路径点坐标视为期望位姿，根据期望位姿与真实位姿的偏差建立阻抗模型，阻抗模型的输入F(s)为机器人末端与约束环境间的作用力，输出E(s)为期待位姿与真实位姿的偏差，借助阻抗模型计算出探头坐标系切向的速度以及探头的角速度分量，轨道控制器模块根据机器人末端工具的位移偏差与力偏差的动态关系实现力位协调控制，基于位置的阻抗控制器为阻抗设定期望阻抗参数M、D和K，其中M代表惯性矩阵，D代表阻尼矩阵，K代表刚度矩阵，通过调节期望的阻抗参数改变动态特性，减小机器人末端与超声探头的轨迹偏差与接触力偏差，保证系统处于过阻尼或临界阻尼状态，防止超调现象。

进一步地，上位机中构建有自适应力控制器模块；

自适应力控制器模块根据约束环境动态调整阻尼参数ΔD(t)，根据力误差校正后的公式如下：

其中，D为阻尼系数，F_e和F_d分别为接触力和期望接触力，当环境为复杂平面或斜坡时，F_e(t)和F_d(t)是时变的函数，分别指的是t时刻的接触力；在超声扫描过程中，

和z分别为探头在z轴方向的速度和加速度，

和z_e分别为探头的初始环境速度和加速度，定义e＝z-z_e，

当环境为复杂平面或斜坡时，z_e是一个时变函数；定义环境预估值

此时轨迹误差表示为

定义

当环境为复杂平面或斜坡时，

为时变函数，代表t时刻的

值；σ为更新率，Δt为自适应阻抗控制的采样率，代表机器人阻抗系统在z轴方向的刚度系数；通过动态调整阻尼系数可以根据扫描过程中真实接触力与期望接触力的偏差计算得到沿超声探头法线的速度分量。

进一步地，上位机中构建有超声视觉控制器模块；

超声视觉控制器模块通过对扫描得到的超声图像进行置信度估计，根据超声图像两侧的整体置信度差异调整机器人末端的超声探头在超声束平面内的旋转；

首先通过随机游走算法将检测图像作为由各像素结点和其连接边构成的无向连通带权图，计算每个像素到种子结点之一的超声置信度，超声图像平均置信度可以反映探头与待测组织的接触情况；

在超声扫描过程中，机器人末端的接触力F_e与采集到的超声图像平均置信度C_m都小于触发阈值，系统仅进行位置控制；而当探头与人体组织接触时，脱接触力F_e大于设定的触发阈值F_min则启动力自适应控制器，当真实接触力与期望接触力的偏差Δf大于设定的阈值F_ε，力自适应控制器根据力偏差Δf计算超声探头发现方向的速度分量；而当实时扫描的超声图像的整体平均置信度C_m大于设定的阈值s_C时，诗句控制器计算超声探头的角速度修正量

通过力自适应控制器与超声视觉控制器计算得到的超声探头速度修正量对轨道控制器得到的探头速度进行更新，将其转化到机器人基坐标系下。

进一步地，上位机中构建有超声扫描核心控制模块；

超声扫描核心控制模块是整个扫描控制系统的核心，负责整个超声扫描过程中的各模块运行，通过控制轨迹控制器模块与坐标系转化模块的驱动并控制超声探头坐标系切向位移以及探头姿态控制，通过管理超声视觉控制器模块控制超声探头从而向上位机上反馈超声图像，通过管理自适应力控制器模块与力传感器补偿模块的驱动计算超声探头和待检测目标的接触力大小并计算速度控制硬件层的力传感器的工作。

进一步地，超声扫描控制模块负责监视器显示的启动与硬件层的机器人的运动状态，实现上位机与相机、超声探头、机器人、六维力传感器和监视器之间的数据通信，实现机器人的运动控制。

进一步地，B超探头通过夹具固定在机器人末端，像上位机实时反馈扫描的超声图像信息并在监视器中显示。

相比于现有技术，本发明的优点在于：

1.在上位机中构建的基于自适应阻抗控制策略在探头法向上建立自适应力控制器，该控制器可以根据环境信息的变化自适应调整阻抗模型的阻尼系数，提高在复杂环境中的抗干扰能力；

2.在上位机中构建的基于随机游走算法对超声图像进行置信度估计，搭建的超声视觉伺服控制器提取超声图像中的特征并将其转化为机器人的控制指令，基于超声图像的反馈自动调整机器人末端探头在超声束平面内的旋转，实现超声扫描的闭环控制。

附图说明

图1是本发明实施例中一种基于多模态融合的超声扫描控制系统的结构示意图；

图2是本发明实施例中一种基于多模态融合的超声扫描控制系统的硬件平台示意图。

图3是本发明实施例中一种基于多模态融合的超声扫描控制系统的软件设计结构示意图。

图4是本发明实施例中六维力传感器标定示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1：

一种基于多模态融合的超声扫描控制系统，包括相机、超声探头、机器人、力传感器和上位机；

其中，力传感器和超声探头均固定于机器人末端，相机、力传感器和超声探头均连接上位机并向其传输待检测目标的色彩深度信息、接触力信息和超声图像；上位机根据接收的信息向机器人发送控制指令；机器人连接上位机并根据上位机发送的指令进行运动并反馈自身运动状态。

进一步地，力传感器和超声探头通过夹具固定于机器人末端，其中，超声探头刚性固定于机器人末端；力传感器一端连接夹具上端，另一端连接机器人末端，获取超声探头和待检测目标的接触力大小。

进一步地，上位机中构建有坐标系转换模块；

机器人初始化阶段，坐标系转换模块通过手眼标定算法建立相机坐标系和机器人基坐标系之间的位姿变换关系，将上位机中提前规划好的超声扫描点空间坐标从相机坐标系下变换到机器人基坐标系。

进一步地，上位机中构建有力传感器重力补偿模块；

由于夹具和超声探头本身的重力，在进行力控制之前需要对其进行补偿，力传感器重力补偿模块在系统初始化前需要计算超声探头及夹具的重力以及力传感器的零点偏移值，对力传感器测量得到的力值数据进行补偿，最终计算得到真实的接触力；

六维力传感器标定示意图如4所示；

设定六维力传感器的测量值为F＝[F_x,F_y,F_z,M_x,M_y,M_z]，其中六维力传感器所测量的力在六维力传感器坐标轴x,y,z轴上的分量为(F_x,F_y,F_z)与其在在六维力传感器坐标轴x,y,z轴上的力矩分量为(M_x,M_y,M_z)；零点值为

由探头和夹具组成的末端工具在该六维力传感器坐标系下的力和力矩为

末端工具重力G在六维力传感器坐标轴上的力矩分量

由下列公式可得：

其中(x_o,y_o,z_o)是末端工具质心在六维力传感器坐标系中的空间坐标；在静态标定时无外部作用力作用在末端工具上，

因此，末端工具与环境的真实接触力

的公式为：

进一步地，上位机中构建有轨迹控制器模块；

轨迹控制器模块接收上位机发送的超声扫描点空间坐标，将接收的超声扫描路径点坐标视为期望位姿，根据期望位姿与真实位姿的偏差建立阻抗模型，阻抗模型的输入F(s)为机器人末端与约束环境间的作用力，输出E(s)为期待位姿与真实位姿的偏差，借助阻抗模型计算出探头坐标系切向的速度以及探头的角速度分量，轨道控制器模块根据机器人末端工具的位移偏差与力偏差的动态关系实现力位协调控制，基于位置的阻抗控制器为阻抗设定合理的期望阻抗参数M，D，K，其中M代表惯性矩阵，D代表阻尼矩阵，K代表刚度矩阵，通过调节期望的阻抗参数改变动态特性，减小机器人末端与超声探头的轨迹偏差与接触力偏差，保证系统处于过阻尼或临界阻尼状态，防止超调现象。

进一步地，上位机中构建有自适应力控制器模块；

自适应力控制器模块根据约束环境动态调整阻尼参数ΔD(t)，根据力误差校正后的公式如下：

其中，D为阻尼系数，F_e和F_d分别为接触力和期望接触力，当环境为复杂平面或斜坡时，F_e(t)和F_d(t)是时变的函数，分别指的是t时刻的接触力；在超声扫描过程中，

和z分别为探头在z轴方向的速度和加速度，

和z_e分别为探头的初始环境速度和加速度，定义e＝z-z_e，

当环境为复杂平面或斜坡时，z_e是一个时变函数；定义环境预估值

此时轨迹误差表示为

定义

当环境为复杂平面或斜坡时，

为时变函数，代表t时刻的

值；σ为更新率，Δt为自适应阻抗控制的采样率，代表机器人阻抗系统在z轴方向的刚度系数；通过动态调整阻尼系数可以根据扫描过程中真实接触力与期望接触力的偏差计算得到沿超声探头法线的速度分量。

进一步地，上位机中构建有超声视觉控制器模块；

超声视觉控制器模块通过对扫描得到的超声图像进行置信度估计，根据超声图像两侧的整体置信度差异调整机器人末端的超声探头在超声束平面内的旋转；

首先通过随机游走算法将检测图像作为由各像素结点和其连接边构成的无向连通带权图，计算每个像素到种子结点之一的超声置信度，超声图像平均置信度可以反映探头与待测组织的接触情况；

在超声扫描过程中，机器人末端的接触力F_e与采集到的超声图像平均置信度C_m都小于触发阈值，系统仅进行位置控制；而当探头与人体组织接触时，脱接触力F_e大于设定的触发阈值F_min则启动力自适应控制器，当真实接触力与期望接触力的偏差Δf大于设定的阈值F_ε，力自适应控制器根据力偏差Δf计算超声探头发现方向的速度分量；而当实时扫描的超声图像的整体平均置信度C_m大于设定的阈值s_C时，诗句控制器计算超声探头的角速度修正量

通过力自适应控制器与超声视觉控制器计算得到的超声探头速度修正量对轨道控制器得到的探头速度进行更新，将其转化到机器人基坐标系下。

进一步地，上位机中构建有超声扫描核心控制模块；

超声扫描核心控制模块是整个扫描控制系统的核心，负责整个超声扫描过程中的各模块运行，通过控制轨迹控制器模块与坐标系转化模块的驱动并控制超声探头坐标系切向位移以及探头姿态控制，通过管理超声视觉控制器模块控制超声探头从而向上位机上反馈超声图像，通过管理自适应力控制器模块与力传感器补偿模块的驱动计算超声探头和待检测目标的接触力大小并计算速度控制硬件层的力传感器的工作。

进一步地，超声扫描控制模块负责监视器显示的启动与硬件层的机器人的运动状态，实现上位机与相机、超声探头、机器人、力传感器和监视器之间的数据通信，实现机器人的运动控制。

进一步地，B超探头通过夹具固定在机器人末端，向上位机实时反馈扫描的超声图像信息并在监视器中显示。

本实施例中，参见图2，本发明实施例提供一种超声自动化扫描系统硬件平台，主要包括：(1)实时获取周围环境RGB-D影像的Azure Kinect传感器(相机)；(2)六自由度机器人Universal Robots UR3及其固定平台(超声机器人)；(3)高精度六维力传感器及家具(力传感器)；(4)Cprobe系列医学超声成像设备(超声探头)。

Azure Kinect视觉传感器用于获取待检测目标的色彩深度信息，通过USB 3.0接口与上位机进行数据传输。视觉传感器还可以提供视觉反馈，便于医师通过交互界面实时观察机器人的运动状态；UR3机器人与上位机之间通过以太网接口进行通讯，机器人根据上位机发送的脚本命令进行运动，同时向上位机反馈自身的运动状态信息；六维力传感器安装于UR3机器人末端，与上位机通过USB 3.0接口互联，向上位机传递测得的接触力信息；B超探头通过夹具固定于机器人末端，通过WIFI无线传输向上位机实时反馈扫描得到的超声图像信息。

本实施例中，选取高频线阵探头用于甲状腺超声扫查，探头型号为SonoStar的CProbe系列全数字超声显像诊断仪，传统的超声成像设备需要接触于线缆将探头末端与主机设备连接起来，实施例中使用的新一代超声成像设备将主机电路集成到了探头内部，并通过WIFI无线传输技术将扫描得到的图像传输到监视器上进行显示，保证较高的成像精度。

本实施例中，使用的机器人为丹麦Universal Robots公司的UR3机器人，该机器人的每个腕关节都可以绕旋转轴实现三百六十度旋转，并且机器人末端关节可以无限旋转，支持TCP/IP通信协议，可以通过连接远程端口的形式控制机器人，获取机器人的运动状态信息。

本实施例中，所述高精度力传感器型号为MIOS-Y80-H45六维力传感器，基于实验中用到的探头尺寸和UR3机器人尺寸参数，通过SOLIDWORKS设计了探头夹具来讲B超探头刚性固定到机器人地段，夹具内部装有弹簧装置用于减小因过失操作引起的探头损坏。夹具上端连接六维力传感器，六维力传感器与机器人末端相连。

参见图3，本实施例中，一种基于多模态融合的超声扫描控制系统从软件结构分为硬件层，功能层和UI层。各模块介绍如下：

1)坐标系转换模块

在机器人初始化阶段，该模块通过手眼标定算法建立Azure Kinect相机坐标系和机器人基坐标系间的位姿变换关系，将上位机中规划号的超声扫描点空间坐标从AzureKinect相机坐标系下变换到机器人基坐标系。

2)力传感器重力补偿模块

由于探头夹具和探头本身的重力，在进行力控制之前需要对其进行补偿，该模块在系统初始化前需要计算探头及夹具的重力以及传感器的零点偏移值，对六维力传感器测量得到的力值数据进行补偿，最终计算得到真实的接触力。

3)轨迹控制器模块

该模块可以接收上位机发送的超声扫描路径点坐标，将接收的扫描点坐标视为期望位姿，得到期望位姿与真实位姿间的偏差后根据建立的阻抗模型计算出探头坐标系切向的速度以及探头的角速度分量。

4)自适应力控制器模块

该模块可以根据约束环境动态调整阻尼系数,负责根据扫描过程中真实接触力与期望接触力的偏差计算得到沿探头法线的速度分量。

5)超声视觉控制器模块

该模块通过对扫描得到的超声图像进行置信度估计,根据图像两侧的整体置信度差异调整机器人末端探头在超声束平面内的旋转。

6)超声扫描核心控制模块

该模块是整个扫描控制系统的核心，负责超声扫描过程中的逻辑控制,各设备间的数据通信，以及机器人的运动控制。

实施例2：

与实施例1不同之处在于对于超声图像受斑点噪声影响的现象，采用维纳滤波器对原始超声图像进行滤波，对原始超声图像和采用维纳滤波器进行滤波处理后的超声图像的图像强度平均值与标准差来作为超声图像的噪声特征；

对于过大的接触力在超声图像采集过程中造成的患者不适与相关组织变形从而导致的错误的临床判断，使用了一种压缩特征，即在超声图像的竖直方向上，亮度高于阈值p_ε的像素的最差值和最小值相对于图像大小的差值；

通过构建不同接触力下超声图像与成像质量的集合，通过超声图像所提取的三个特征，即超声置信度，噪声特征与压缩特征作为孪生支持向量机(TSVM)的输入模型，样本类标签为图像质量，将输入特征进行归一化处理后输入到TSVM模型中进行训练，通过径向基函数将其映射到高维空间实现对其线性分类，对超声图像进行质量评估。

实施例3：

与实施例2不同的是将基于多模态融合的超声扫描控制方法应用于人体甲状腺超声扫描时间中，在超声视觉控制器的激活阈值设置为C_min＝0.1，C_max＝0.5。力控制器设置的最小阈值F_min＝0.5N，期望接触力F_d＝5N。

以上所述是本发明的优选实施方式。应当指出，对于本技术领域的普通人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.一种基于多模态融合的超声扫描控制系统，其特征在于，包括相机、超声探头、机器人、力传感器和上位机；

其中，力传感器和超声探头均固定于机器人末端，相机、力传感器和超声探头均连接上位机并向其传输待检测目标的色彩深度信息、接触力信息和超声图像；上位机根据接收的信息向机器人发送控制指令；机器人连接上位机并根据上位机发送的指令进行运动并反馈自身运动状态。

2.根据权利要求1所述的一种基于多模态融合的超声扫描控制系统，其特征在于，力传感器和超声探头通过夹具固定于机器人末端，其中，超声探头刚性固定于机器人末端；力传感器一端连接夹具上端，另一端连接机器人末端，获取超声探头和待检测目标的接触力大小。

3.根据权利要求1所述的一种基于多模态融合的超声扫描控制系统，其特征在于，上位机中构建有坐标系转换模块；

机器人初始化阶段，坐标系转换模块通过手眼标定算法建立相机坐标系和机器人基坐标系之间的位姿变换关系，将上位机中提前规划好的超声扫描点空间坐标从相机坐标系下变换到机器人基坐标系。

4.根据权利要求1所述的一种基于多模态融合的超声扫描控制系统，其特征在于，上位机中构建有力传感器重力补偿模块；

由于夹具和超声探头本身的重力，在进行力控制之前需要对其进行补偿，力传感器重力补偿模块在系统初始化前需要计算超声探头及夹具的重力以及力传感器的零点偏移值，对力传感器测量得到的力值数据进行补偿，最终计算得到真实的接触力；

六维力传感器所测量的力在六维力传感器坐标轴x,y,z轴上的分量为(F_x,F_y,F_z)，在六维力传感器坐标轴x,y,z轴上的力矩分量为(M_x,M_y,M_z)，设定六维力传感器的测量值为F＝[F_x,F_y,F_z,M_x,M_y,M_z]；零点值为

由探头和夹具组成的末端工具在该六维力传感器坐标系下的力和力矩为

末端工具重力G在六维力传感器坐标轴上的力矩分量

由下列公式可得：

其中(x_o,y_o,z_o)是末端工具质心在六维力传感器坐标系中的空间坐标；在静态标定时无外部作用力作用在末端工具上，

因此，末端工具与环境的真实接触力

的公式为：

5.根据权利要求1所述的一种基于多模态融合的超声扫描控制系统，其特征在于，上位机中构建有轨迹控制器模块；

轨迹控制器模块接收上位机发送的超声扫描点空间坐标，将接收的超声扫描路径点坐标视为期望位姿，根据期望位姿与真实位姿的偏差建立阻抗模型，阻抗模型的输入F(s)为机器人末端与约束环境间的作用力，输出E(s)为期待位姿与真实位姿的偏差，借助阻抗模型计算出探头坐标系切向的速度以及探头的角速度分量，轨道控制器模块根据机器人末端工具的位移偏差与力偏差的动态关系实现力位协调控制，基于位置的阻抗控制器为阻抗设定期望阻抗参数M、D和K，其中M代表惯性矩阵，D代表阻尼矩阵，K代表刚度矩阵，通过调节期望的阻抗参数改变动态特性，减小机器人末端与超声探头的轨迹偏差与接触力偏差，保证系统处于过阻尼或临界阻尼状态，防止超调现象。

6.根据权利要求1所述的一种基于多模态融合的超声扫描控制系统，其特征在于，上位机中构建有自适应力控制器模块；

自适应力控制器模块根据约束环境动态调整阻尼参数ΔD(t)，根据力误差校正后的公式如下：

其中，D为阻尼系数，F_e和F_d分别为接触力和期望接触力，当环境为复杂平面或斜坡时，F_e(t)和F_d(t)是时变的函数，分别指的是t时刻的接触力和期望接触力；在超声扫描过程中，

和z分别为探头在z轴方向的速度和加速度，

和z_e分别为探头的初始环境速度和加速度，定义e＝z-z_e，

当环境为复杂平面或斜坡时，z_e是一个时变函数；定义环境预估值

此时轨迹误差表示为

定义

当环境为复杂平面或斜坡时，

为时变函数，代表t时刻的

值；σ为更新率，Δt为自适应阻抗控制的采样率，代表机器人阻抗系统在z轴方向的刚度系数；通过动态调整阻尼系数可以根据扫描过程中真实接触力与期望接触力的偏差计算得到沿超声探头法线的速度分量。

7.根据权利要求1所述的一种基于多模态融合的超声扫描控制系统，其特征在于，上位机中构建有超声视觉控制器模块；

超声视觉控制器模块通过对扫描得到的超声图像进行置信度估计，根据超声图像两侧的整体置信度差异调整机器人末端的超声探头在超声束平面内的旋转；

首先通过随机游走算法将检测图像作为由各像素结点和其连接边构成的无向连通带权图，计算每个像素到种子结点之一的超声置信度，超声图像平均置信度可以反映探头与待测组织的接触情况；

在超声扫描过程中，机器人末端的接触力F_e与采集到的超声图像平均置信度C_m都小于触发阈值，系统仅进行位置控制；而当探头与人体组织接触时，脱接触力F_e大于设定的触发阈值F_min则启动力自适应控制器，当真实接触力与期望接触力的偏差Δf大于设定的阈值F_ε，力自适应控制器根据力偏差Δf计算超声探头发现方向的速度分量；而当实时扫描的超声图像的整体平均置信度C_m大于设定的阈值s_C时，诗句控制器计算超声探头的角速度修正量

通过力自适应控制器与超声视觉控制器计算得到的超声探头速度修正量对轨道控制器得到的探头速度进行更新，将其转化到机器人基坐标系下。

8.根据权利要求1所述的一种基于多模态融合的超声扫描控制系统，其特征在于，上位机中构建有超声扫描核心控制模块；

超声扫描核心控制模块是整个扫描控制系统的核心，负责整个超声扫描过程中的各模块运行，通过控制轨迹控制器模块与坐标系转化模块的驱动并控制超声探头坐标系切向位移以及探头姿态控制，通过管理超声视觉控制器模块控制超声探头从而向上位机上反馈超声图像，通过管理自适应力控制器模块与力传感器补偿模块的驱动计算超声探头和待检测目标的接触力大小并计算速度控制硬件层的力传感器的工作。

9.根据权利要求8所述的一种基于多模态融合的超声扫描控制系统，其特征在于，超声扫描控制模块负责监视器显示的启动与硬件层的机器人的运动状态，实现上位机与相机、超声探头、机器人、六维力传感器和监视器之间的数据通信，实现机器人的运动控制。

10.根据权利要求8所述的一种基于多模态融合的超声扫描控制系统，其特征在于，B超探头通过夹具固定在机器人末端，像上位机实时反馈扫描的超声图像信息并在监视器中显示。

Images