1.一种智慧手术室全局-局部多目标联合定位方法，其特征在于，该方法包括以下步骤：
步骤1：设计一组具有特定几何特征的反光球固定框架，使其在动作捕捉系统与双目光学跟踪系统下均可以被识别；
步骤2：根据固定在手术场景中的多个反光球框架，将场景中的目标坐标系统一为全局表达；
步骤3：通过基于概率遮挡模型的精度估计方法，获得动态手术场景下的概率分布估计，还包括以下步骤：
基于动作捕捉系统的相机参数，确定单相机的可视角度θ，视场距离l；
给定两台动作捕捉相机对空间中视点的可视判据为：

其中为编号为i,j的相机与空间中一点形成视图向量的夹角，φ1，2为定义的可以形成三角视图的阈值，该阈值与相机参数和实验环境相关，其中相机参数包括但不限于相机有效像素和相机焦距内部参数，实验环境包括但不限于实验环境的光线条件和实验使用的反光球尺寸；
当空间中一点与一对相机满足可以形成三角视图关系时，定义垂直于其视图向量张成的平面的无限大遮挡模型，该遮挡物影响相机对空间中一点的观测的概率p为：

则定义给定空间中任一点基于概率遮挡模型的精度度量为：

其中cami为编号为i的相机，n为环境中实际安装的相机台数，得出的value是该点的度量值，通过这种方式，可以得出空间中任意一点的度量值，结合实际测量，可以建立动作捕捉系统工作空间中的精度分布与度量值的映射关系；
步骤4：通过基于卡尔曼滤波的数据融合方法，在手术场景中当反光球框架被部分遮挡时获得连续的定位信息。
2.如权利要求1所述的一种智慧手术室全局-局部多目标联合定位方法，其特征在于，所述的反光球固定框架具有特定的几何特征，在动作捕捉系统与双目光学跟踪系统下均可以被识别。
3.如权利要求1所述的一种智慧手术室全局-局部多目标联合定位方法，其特征在于，所述的固定在手术场景中的多个反光球框架，将场景中的目标坐标系统一为全局表达，其中基坐标系{B}由固定在手术场景中的全局靶标表示，手术器械位姿{tool}在动作捕捉系统和双目光学跟踪系统中可以分别表示为：

其中，{M}、{N}指动作捕捉系统和双目光学跟踪系统的基坐标系，T指空间位姿的旋转矩阵，这样可以得到统一在基坐标系下的全局坐标表达。
4.如权利要求1所述的一种智慧手术室全局-局部多目标联合定位方法，其特征在于，所述的通过基于卡尔曼滤波的数据融合方法，在手术场景中当反光球框架被部分遮挡时获得连续的定位信息；
在手术室场景中，以固定的时间周期T同步采集动作捕捉系统与双目光学跟踪系统的数据，并使用卡尔曼滤波对数据进行融合，考虑到动作捕捉系统标定中存在系统误差，该误差会使测量值与空间位置存在非线性但局部连续的偏差，而双目光学系统在局部精度相对可靠，故对系统采用扩展的误差模型：
yk＝pk+bk+wk
其中yk为k时刻的测量值，pk为k时刻的实际位置，wk为k时刻的噪声，在此基础上定义扩展的状态向量：
xk＝[px py pz vx vy vz bx by bz]T
其中xk定义为k时刻目标状态，px、py、pz分别为目标在x、y、z方向上的位置，vx、vy、vz分别为目标在x、y、z方向上的速度，bx、by、bz分别为目标在x、y、z方向上测量上的偏差。

技术领域
[0001]本发明属于机器人手术领域，尤其涉及一种智慧手术室全局-局部多目标联合定位方法。
背景技术
[0002]近年来，随着人工智能技术、机器人技术的发展，精准化、数字化、智能化、个性化的智能医疗手段不断出现，进而提出了智慧手术室的概念。在智能化的手术场景中，可以实现机器人参与手术流程，如医生操作机器人进行手术，或机器人按照术前规划进行手术。在这样的手术过程中，需要掌握手术场景内的目标，如手术器械、机械臂等的实时位置，以确保手术进程的安全。
[0003]在现有的机器人手术场景中，通常使用局部定位的方法，即通过视觉传感器获得手术部位与手术器械的相对位置。尽管相关方法已经比较成熟，但仍存在一些挑战。具体来说，所有局部定位的方法都无法规避遮挡问题，当传感器与目标之间出现遮挡时，需要暂时中断手术进程，调整传感器位置或者姿态，待确保遮挡不再影响手术操作后继续手术进程。这里存在一些问题：首先，手术室内分秒必争，中断手术进程有可能造成对患者的伤害；其次，手术室作为一个人机共融的场景，必然出现大量随机、动态的遮挡，当遮挡出现时，不能丢失手术器械等目标位置的信息；最后，智慧手术室中存在其他的移动目标，需要时刻掌握他们的全局位置。因此，单纯的局部定位不能满足智慧手术室的需求。
[0004]对于全局场景下目标的定位，动作捕捉系统广泛应用于各类场景中。动作捕捉系统是一个多相机系统，其精度被认为是可靠的。在其他应用场景中，通常会避免出现遮挡，在开阔的场景中进行应用；但在人机共融的智慧手术室场景中，天然存在着大量动态、静态的遮挡，多目标的移动也会产生互相遮挡。在动态场景下，动作捕捉系统的精度的评价缺乏标准。
[0005]因此，提出一种智慧手术室全局-局部多目标联合定位方法，以解决上述问题。
发明内容
[0006]为了解决上述技术问题，本发明提供一种智慧手术室全局-局部多目标联合定位方法，该方法包括以下步骤：
[0007]步骤1：设计一组具有特定几何特征的反光球固定框架，使其在动作捕捉系统与双目光学跟踪系统下均可以被识别；
[0008]步骤2：根据固定在手术场景中的多个反光球框架，将场景中的目标坐标系统一为全局表达；
[0009]步骤3：通过基于概率遮挡模型的精度估计方法，获得动态手术场景下的概率分布估计；
[0010]步骤4：通过基于卡尔曼滤波的数据融合方法，在手术场景中当反光球框架被部分遮挡时获得连续的定位信息。
[0011]优选的，所述的一种智慧手术室全局-局部多目标联合定位方法，所述的反光球固定框架具有特定的几何特征，在动作捕捉系统与双目光学跟踪系统下均可以被识别。
[0012]优选的，所述的一种智慧手术室全局-局部多目标联合定位方法，所述的固定在手术场景中的多个反光球框架，将场景中的目标坐标系统一为全局表达，其中基坐标系{B}由固定在手术场景中的全局靶标表示，手术器械位姿{tool}在动作捕捉系统和双目光学跟踪系统中可以分别表示为：
[0013]
[0014]
[0015]其中，{M}、{N}指动作捕捉系统和双目光学跟踪系统的基坐标系，T指空间位姿的旋转矩阵，这样可以得到统一在基坐标系下的全局坐标表达。
[0016]优选的，所述的一种智慧手术室全局-局部多目标联合定位方法，所述的基于概率遮挡模型的精度估计方法，获得动态手术场景下的概率分布估计，主要步骤包括：
[0017]基于动作捕捉系统的相机参数，确定单相机的可视角度θ，视场距离l。
[0018]给定两台动作捕捉相机对空间中视点的可视判据为：
[0019]
[0020]其中为编号为i,j的相机与空间中一点形成视图向量的夹角，φ1，2为定义的可以形成三角视图的阈值，该阈值与相机参数(包括但不限于相机有效像素、相机焦距等相机内部参数)、实验环境(包括但不限于实验环境的光线条件，实验使用的反光球尺寸等外部参数)相关。
[0021]当空间中一点与一对相机满足可以形成三角视图关系条件时，定义垂直于其视图向量张成的平面的无限大遮挡模型，该遮挡物影响相机对空间中一点的观测的概率为：
[0022]
[0023]则定义给定空间中任一点基于概率遮挡模型的精度度量为：
[0024]
[0025]其中cami为编号为i的相机，n为环境中实际安装的相机台数，得出的value是该点的度量值。通过这种方式，可以得出空间中任意一点的度量值，结合实际测量，可以建立动作捕捉系统工作空间中的精度分布与度量值的映射关系。
[0026]优选的，所述的一种智慧手术室全局-局部多目标联合定位方法，其特征在于，所述的通过基于卡尔曼滤波的数据融合方法，融合动作捕捉系统与双目光学跟踪系统的定位信息、在手术场景中当反光球框架被部分遮挡时获得连续的定位信息。
[0027]在手术室场景中，以固定的时间周期T同步采集动作捕捉系统与双目光学跟踪系统的数据，并使用卡尔曼滤波对数据进行融合。考虑到动作捕捉系统标定中存在系统误差，该误差会使测量值与空间位置存在非线性但局部连续的偏差，而双目光学系统在局部精度相对可靠。故对系统采用扩展的误差模型：
[0028]yk＝pk+bk+wk
[0029]其中yk为k时刻的测量值，pk为k时刻的实际位置，wk为k时刻的噪声。在此基础上定义扩展的状态向量：
[0030]xk＝[px py pz vx vy vz bx by bz]T
[0031]其中xk定义为k时刻目标状态，px、py、pz分别为目标在x、y、z方向上的位置，vx、vy、vz分别为目标在x、y、z方向上的速度，bx、by、bz分别为目标在x、y、z方向上测量上的偏差。
[0032]与现有技术相比，本发明的有益效果为：
[0033](1)本发明使用具有特定几何规则的反光球框架，可以得到统一在基坐标系下的全局坐标表达。
[0034](2)本发明使用基于概率遮挡模型的精度估计方法，面向动态、随机的智慧手术室人机共融场景，提出了一种评价动作捕捉系统精度的可行方法。
[0035](3)本发明使用基于卡尔曼滤波的数据融合方法，结合动作捕捉系统与双目光学跟踪系统进行全局-局部联合定位，可以规避手术过程中出现遮挡导致中断手术进程的问题，并且使用扩展的状态模型，将测量中产生的偏差估计并进行补偿，可以有效提升定位准确度。
附图说明
[0036]图1是本发明的系统的应用场景示意图。
[0037]图2是本发明的具有特定几何特征的反光球框架示意图。
[0038]图3是本发明的基于概率遮挡模型的精度评价结果示意图。
[0039]图4是本发明的基于卡尔曼滤波的数据融合结果示意图。
[0040]图中：①、动作捕捉系统；②双目光学跟踪系统。
具体实施方式
[0041]以下结合附图对本发明做进一步描述：
[0042]实施例：
[0043]如图1所示，利用已有实验室进行一种智慧手术室全局-局部多目标联合定位方法实验。该方法包括以下步骤：
[0044]步骤1：设计一组具有特定几何特征的反光球固定框架，使其在动作捕捉系统与双目光学跟踪系统下均可以被识别；
[0045]步骤2：根据固定在手术场景中的多个反光球框架，将场景中的目标坐标系统一为全局表达；
[0046]步骤3：通过基于概率遮挡模型的精度估计方法，获得动态手术场景下的概率分布估计；
[0047]步骤4：通过基于卡尔曼滤波的数据融合方法，在手术场景中当反光球框架被部分遮挡时获得连续的定位信息。
[0048]具体的的，所述的一种智慧手术室全局-局部多目标联合定位方法，所述的反光球固定框架具有特定的几何特征，如图2所示，该反光球固定框架在动作捕捉系统与双目光学跟踪系统下均可以被识别。
[0049]具体的，所述的一种智慧手术室全局-局部多目标联合定位方法，所述的固定在手术场景中的多个反光球框架，将场景中的目标坐标系统一为全局表达，其中基坐标系{B}由固定在手术场景中的全局靶标表示，手术器械位姿{tool}在动作捕捉系统和双目光学跟踪系统中可以分别表示为：
[0050]
[0051]
[0052]其中，{M}、{N}指动作捕捉系统①和双目光学跟踪系统②的基坐标系，T指空间位姿的旋转矩阵，这样可以得到统一在基坐标系下的全局坐标表达。
[0053]具体的，所述的一种智慧手术室全局-局部多目标联合定位方法，所述的基于概率遮挡模型的精度估计方法，获得动态手术场景下的概率分布估计，如图3所示。主要步骤包括：
[0054]基于动作捕捉系统的相机参数，确定单相机的可视角度θ，视场距离l。
[0055]给定两台动作捕捉相机对空间中视点的可视判据为：
[0056]
[0057]其中为编号为i,j的相机与空间中一点形成视图向量的夹角，φ1，2为定义的可以形成三角视图的阈值，该阈值与相机参数(包括但不限于相机有效像素、相机焦距等相机内部参数)、实验环境(包括但不限于实验环境的光线条件，实验使用的反光球尺寸等外部参数)相关。在已有的实验环境中，φ1＝40°，φ2＝140°，即目标点与相机形成视图向量夹角在(40°,140°)之内时，认为目标点是可见的。
[0058]当空间中一点与一对相机满足可以形成三角视图关系条件时，定义垂直于其视图向量张成的平面的无限大遮挡模型，该遮挡物影响相机对空间中一点的观测的概率为：
[0059]
[0060]则定义给定空间中任一点基于概率遮挡模型的精度度量为：
[0061]
[0062]其中cami为编号为i的相机，n为环境中实际安装的相机台数，得出的value是该点的度量值。通过这种方式，可以得出空间中任意一点的度量值，结合实际测量，建立出动作捕捉系统工作空间中的精度分布与度量值的映射关系。
[0063]具体的，所述的一种智慧手术室全局-局部多目标联合定位方法，其特征在于，所述的通过基于卡尔曼滤波的数据融合方法，融合动作捕捉系统与双目光学跟踪系统的定位信息、在手术场景中当反光球框架被部分遮挡时获得连续的定位信息。
[0064]在手术室场景中，以固定的时间周期T同步采集动作捕捉系统①与双目光学跟踪系统②的数据，并使用卡尔曼滤波对数据进行融合。考虑到动作捕捉系统标定中存在系统误差，该误差会使测量值与空间位置存在非线性但局部连续的偏差，而双目光学系统在局部精度相对可靠。故对系统采用扩展的误差模型：
[0065]yk＝pk+bk+wk
[0066]其中yk为k时刻的测量值，pk为k时刻的实际位置，wk为k时刻的噪声。在此基础上定义扩展的状态向量：
[0067]xk＝[px py pz vx vy vz bx by bz]T
[0068]其中xk定义为k时刻目标状态，px、py、pz分别为目标在x、y、z方向上的位置，vx、vy、vz分别为目标在x、y、z方向上的速度，bx、by、bz分别为目标在x、y、z方向上测量上的偏差。由于实际实验中目标运动平稳，在局部内可认为是匀速运动模型，则状态转移矩阵为：
[0069]
[0070]其中T为上文中所述的采样周期。
[0071]在实验中，定义动作捕捉系统的观测矩阵为：
[0072]HMocap＝[I3 0 I3]
[0073]双目跟踪系统的观测矩阵为：
[0074]HOTS＝[I3 0 0]
[0075]其中I3为三阶的单位矩阵。
[0076]则从传感器可以获得数据的k时刻开始，初始化状态向量xk，由步骤三获得的结果初始化方差矩阵Pk，则该时刻对经过采样周期T(在该实施例中传感器采样频率为60Hz，则T＝1/60秒)后k+1时刻的状态预测为：
[0077]xk+1|k＝Axk
[0078]该时刻对k+1时刻的误差预测为：
[0079]Pk+1|k＝APkAT+Qk
[0080]其中Qk为过程噪声矩阵。
[0081]该时刻对k+1时刻的观测预测为：
[0082]yk+1|k＝Hxk+1|k
[0083]其中H是根据实际采样结果选择的观测矩阵，即若数据源是动作捕捉，则H＝HMocap；数据源是光学追踪系统，则H＝HOTS。
[0084]根据以上结果构造卡尔曼滤波器：
[0085]
[0086]其中Rk为由步骤3确定的观测误差矩阵。
[0087]根据实际采样结果yk，可以确定对k+1时刻经卡尔曼滤波器修正后的状态xk+1：
[0088]xk+1＝xk+1|k+Kk(yk+1-yk+1|k)
[0089]与下次迭代使用的协方差矩阵：
[0090]Pk+1＝Pk+1|k-KkHPk+1|k
[0091]进行10次独立实验取平均，所提方法与真实轨迹的均方根误差为0.01023，传统卡尔曼滤波为0.01130；为模拟遮挡环境，在数据中随机抽取30％数据置为0，即认为在这些数据所在的位置发生了遮挡，所提方法与真实轨迹的均方根误差为0.01246，传统卡尔曼滤波为0.01282。结果均好于传统方法。
[0092]联合定位效果如图4所示。
[0093]在本发明的描述中，需要理解的是，术语“同轴”、“底部”、“一端”、“顶部”、“中部”、“另一端”、“上”、“一侧”、“顶部”、“内”、“前部”、“中央”、“两端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0094]此外，术语“第一”、“第二”、“第三”、“第四”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量，由此，限定有“第一”、“第二”、“第三”、“第四”的特征可以明示或者隐含地包括至少一个该特征。
[0095]在本发明中，除非另有明确的规定和限定，术语“安装”、“设置”、“连接”、“固定”、“旋接”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
[0096]尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及等同物限定。