一种光学与惯性融合的定位跟踪方法及装置制造方法及图纸

本发明专利技术提供一种光学与惯性融合的定位跟踪方法及装置，方法包括如：A、设置融合定位模块、惯性定位模块；B、计算频率增强参数K；C、部署融合定位模块和训练好后的惯性定位模块；D、若同时有光学数据和惯性数据，则进步骤E；若仅具有惯性数据，则进步骤F；E、将光学数据和惯性数据作为第一滤波器的输入，进步骤D；F、若在之前的K+1个时刻均无光学数据，则进步骤G，否则进步骤H；G、将惯性数据作为第一、第二神经网络的输入，以得到进程噪声矩阵Q

【技术实现步骤摘要】
一种光学与惯性融合的定位跟踪方法及装置


[0001]本专利技术涉及一种光学与惯性融合的定位跟踪方法及装置。

技术介绍

[0002]在一般手术中，医生根据术前获得的患部图像了解病灶的位置和结构，通过记忆和经验进行临床手术。手术的成功取决于临床医生的个人经验和专业技能。作业中的各种参数无法量化，容易造成定位偏差，作业效果往往达不到理想状态。
[0003]立体定向定位系统是计算机辅助手术导航技术的关键。三维定位系统结合医学影像，手术区域以及手术器械联合标定，确定手术目标的空间位置，从而实时获取目标在其三维坐标系中的姿态和位置。目前实现手术导航定位的方法有机械定位、超声波定位、光学定位和电磁定位。
[0004]基于多相机的光学跟踪技术由于其良好的定位精度，在医疗机器人、虚拟现实和工业测量中得到了广泛的应用。其中，光学传感、惯性传感是基本的传感技术。两种技术分别具有独特的优势和不可避免的缺陷。光学传感具有较大的测量范围和最出色的精度，但它受限于视线问题，当目标被遮挡时无法采集数据。惯性传感与其工作空间无关，但它仅在姿态方向上具有相对稳定的测量输出。鉴于各单一模式的缺点，光学和惯性传感的融合由于它们的互补性以及惯性传感的低成本和低功耗而被广泛应用。自动航向参考系统(AHRS)包括三轴陀螺仪和三轴加速度计，具有更稳定的方向和线性加速度输出。随着MEMS(微机电系统)技术的巨大成功，微型MEMS AHRS凭借方向跟踪性能的优势可为光学跟踪系统的提供一个姿态测量的补充。
[0005]但是，尽管此类光学与惯性融合目标跟踪测量技术被广泛应用工业测量和工业自动化中，但实际应用于医疗机器人领域还面临一些其它的挑战，比如光学测量频率不高、跟踪定位连续性不理想、光学测量需要较多的光学标记点以及惯性设备噪声和测量噪声的干扰。

技术实现思路

[0006]本专利技术提出一种光学与惯性融合的定位跟踪方法及装置，能够高精度高可靠度地定位跟踪目标，且具有很强的自适应性、鲁棒性。
[0007]本专利技术通过以下技术方案实现：
[0008]一种光学与惯性融合的定位跟踪方法，包括如下步骤：
[0009]A、设置融合定位模块、惯性定位模块、光学采集模块和惯性采集模块：
[0010]融合定位模块包括级联的第一滤波器和第二滤波器，惯性定位模块包括第一神经网络、第二神经网络、以及输入端分别与第一、第二神经网络输出端连接的第三滤波器，第一、第二、第三滤波器均为无迹卡尔曼滤波器，第一、第二神经网络分别用于估计进程噪声和设备噪声；
[0011]惯性采集模块的采集频率f2是光学采集模块的采集频率f1的整数倍；
[0012]B、计算频率增强参数并将K作为第二滤波器的预测次数；
[0013]C、模拟目标被遮挡的情况，对第一、第二神经网络进行训练，并将融合定位模块和训练好后的惯性定位模块部署在服务器上；
[0014]D、若在当前时刻t，光学采集模块和惯性采集模块同时采集到光学数据和惯性数据，则进入步骤E；若在当前时刻t仅惯性采集模块采集到惯性数据，则进入步骤F；
[0015]E、将当前时刻t的光学数据和惯性数据均作为第一滤波器的输入，得到当前时刻t下目标的运动状态及对应的协方差矩阵，并将t+1时刻作为当前时刻，进入步骤D；
[0016]F、若在当前时刻t之前的K+1个时刻，均无光学采集模块所采集的光学数据，则进入步骤G，否则，进入步骤H；
[0017]G、将惯性采集模块所采集的惯性数据分别作为第一、第二神经网络的输入，以得到进程噪声矩阵Q
t
和设备噪声矩阵R
t
，并将这两个矩阵作为第三滤波器的输入，最终得到当前时刻t下目标的运动状态及对应的协方差矩阵；
[0018]H、将当前时刻t的惯性数据作为第二滤波器的输入，得到当前时刻t下目标的运动状态及对应的协方差矩阵，并将t+1时刻作为当前时刻，进入步骤D。
[0019]进一步的，所述光学数据包括目标的三维位置数据M
t
；所述惯性数据包括目标的三轴线性加速度A
t
。
[0020]进一步的，所述第一神经网络中数据向前传递的计算过程为：
[0021]Y
t
＝SeLU(W
m
×
h
SeLU(W
h
×3A
t
+B
h
×1)+B
m
×1)；
[0022]Z
1t
＝W9×
m
Y
m
×1+B9×1；
[0023]Z
2t
＝W9×
m
Y
m
×1+B9×1；
[0024]Z
3t
＝W9×
m
Y
m
×1+B9×1；
[0025]Z
t
＝diag([Z
1t
,Z
2t
,Z
3t
])；
[0026][0027]其中，W
a
×
b
为第一神经网络中全连接层的a
×
b的权重参数矩阵，B
a
×
b
为该层的a
×
b的偏置矩阵，Z
1t
、Z
2t
和Z
3t
为分别对应于三轴的运动状态的噪声特征估计，SeLU为激活函数。
[0028]进一步的，所述第二神经网络中数据向前传递的计算过程为：
[0029]Y
′
m
×1＝SeLU(W
′
m
×
h
SeLU(W
′
h
×3A
t
+B
′
h
×1)+B
′
m
×1)；
[0030][R1,R2,R3]t
＝W
′3×
m
Y
′
m
×1+B
′3×1；
[0031]R
t
＝abs(diag([R1,R2,R3]t
))；
[0032]其中，W
′
a
×
b
为第二神经网络中全连接层的a
×
b的权重参数矩阵，B
′
a
×
b
为该层的a
×
b的偏置矩阵，R1、R2和R3为分别对应于三轴的运动状态的噪声特征估计，SeLU为激活函数。
[0033]进一步的，所述第三滤波器输出端还与第三神经网络输入端连接，第三滤波器输出的目标在当前时刻下t的运动状态在经第三神经网络后，重建得到当前时刻下t的位移S
″
t
：
[0034]S
″
t
＝W<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种光学与惯性融合的定位跟踪方法，其特征在于：包括如下步骤：A、设置融合定位模块、惯性定位模块、光学采集模块和惯性采集模块：融合定位模块包括级联的第一滤波器和第二滤波器，惯性定位模块包括第一神经网络、第二神经网络、以及输入端分别与第一、第二神经网络输出端连接的第三滤波器，第一、第二、第三滤波器均为无迹卡尔曼滤波器，第一、第二神经网络分别用于估计进程噪声和设备噪声；惯性采集模块的采集频率f2是光学采集模块的采集频率f1的整数倍；B、计算频率增强参数并将K作为第二滤波器的预测次数；C、模拟目标被遮挡的情况，对第一、第二神经网络进行训练，并将融合定位模块和训练好后的惯性定位模块部署在服务器上；D、若在当前时刻t，光学采集模块和惯性采集模块同时采集到光学数据和惯性数据，则进入步骤E；若在当前时刻t仅惯性采集模块采集到惯性数据，则进入步骤F；E、将当前时刻t的光学数据和惯性数据均作为第一滤波器的输入，得到当前时刻t下目标的运动状态及对应的协方差矩阵，并将t+1时刻作为当前时刻，进入步骤D；F、若在当前时刻t之前的K+1个时刻，均无光学采集模块所采集的光学数据，则进入步骤G，否则，进入步骤H；G、将惯性采集模块所采集的惯性数据分别作为第一、第二神经网络的输入，以得到进程噪声矩阵Q
t
和设备噪声矩阵R
t
，并将这两个矩阵作为第三滤波器的输入，最终得到当前时刻t下目标的运动状态及对应的协方差矩阵；H、将当前时刻t的惯性数据作为第二滤波器的输入，得到当前时刻t下目标的运动状态及对应的协方差矩阵，并将t+1时刻作为当前时刻，进入步骤D。2.根据权利要求1所述的一种光学与惯性融合的定位跟踪方法，其特征在于：所述光学数据包括目标的三维位置数据M
t
；所述惯性数据包括目标的三轴线性加速度A
t
。3.根据权利要求2所述的一种光学与惯性融合的定位跟踪方法，其特征在于：所述第一神经网络中数据向前传递的计算过程为：Y
t
＝SeLU(W
m
×
h
SeLU(W
h
×3A
t
+B
h
×1)+B
m
×1)；Z
1t
＝W9×
m
Y
m
×1+B9×1；Z
2t
＝W9×
m
Y
m
×1+B9×1；Z
3t
＝W9×
m
Y
m
×1+B9×1；Z
t
＝diag([Z
1t
,Z
2t
,Z
3t
])；其中，W
a
×
b
为第一神经网络中全连接层的a
×
b的权重参数矩阵，B
a
×
b
为该层的a
×
b的偏置矩阵，Z
1t
、Z
2t
和Z
3t
为分别对应于三轴的运动状态的噪声特征估计，SeLU为激活函数。4.根据权利要求2或3所述的一种光学与惯性融合的定位跟踪方法，其特征在于：所述第二神经网络中数据向前传递的计算过程为：Y
′
m
×1＝SeLU(W
′
m
×
h
SeLU(W
′
h
×3A
t
+B
′
h
×1...

【专利技术属性】
技术研发人员：戴厚德，王增伟，曾雅丹，朱利琦，郑宝志，
申请(专利权)人：泉州装备制造研究所，
类型：发明
国别省市：