[发明专利]一种可穿戴的手术工具定位装置及定位方法

权利要求书

1.一种可穿戴的手术工具定位装置的定位方法，其特征在于，所述方法包括：

通过红外摄像头采集红外灯照射的手术场景，并得到所拍摄手术场景的灰度图像，然后通过红外相机立体定位，得到手术工具上安装的红外反光球的位置，具体包括：通过立体相机三角定位方法，测量出每一个红外反光球相对于红外摄像头坐标系的坐标{x_i,y_i,z_i}，其中i＝1，2，3，4，公式为q＝C*p，q＝{u,v,1}^T，{u,v}为红外反光球在像素坐标系下的坐标，C为相机矩阵，p＝{x,y,z,1}；

通过RGB摄像头采集与所述红外摄像头采集的相同手术场景，并得到所拍摄场景的彩色图像，然后通过RGB相机平面二维码定位，得到手术工具上粘贴的平面二维码的位置，具体包括：通过RGB摄像头识别出平面二维码，使用Aruco定位方法得到平面二维码相对RGB摄像头坐标系的坐标{x,y,z}以及平面二维码的空间姿态；

通过红外摄像头捕捉红外激光发射器发射的点阵，然后利用主动测距的方式获得手术场景的点云图，实现点云定位，具体包括：通过红外摄像头采集红外激光发射器发射的红外点阵，获得手术工具的红外点云，再次得到红外反光球相对于红外摄像头坐标系的z坐标w_j，其中j＝1，2，3，4；

将点云定位得到的手术工具的z轴坐标，与所述红外相机立体定位中通过三角测距得到的手术工具的z轴坐标进行融合，可得到z轴坐标，进而得到手术工具的三维坐标，具体融合方法为：设点云定位得到的手术工具的z轴坐标z₁，三角测距得到的手术工具的z轴坐标z₂，其中z₁,z₂各自依然服从μ₁,μ₂，协方差矩阵Σ_ii的多元高斯分布，通过条件概率分布的原理得到以下结论：

μ_2|1＝μ₂+Σ₂₁Σ^-1₁₁(z₁-μ₁)

Σ_2|1＝Σ₁₁-ΣT₂₁Σ^-1₂₂Σ₂₁

上式即可得到z轴坐标μ_2|1；

将红外相机立体定位、RGB相机平面二维码定位和点云定位三者的定位结果进行融合和坐标转换，得到手术工具的位置与姿态。

2.根据权利要求1所述的定位方法，其特征在于，所述RGB相机平面二维码定位是基于Aruco定位方法，先获取所述平面二维码与所述RGB摄像头的相对位姿，再通过已知的平面二维码与手术工具执行端的位置关系，换算出手术工具执行端相对所述RGB摄像头的位姿。

3.根据权利要求1所述的定位方法，其特征在于，所述红外相机立体定位基于立体相机三角定位方法，先获取红外反光球相对于红外摄像头的位置关系，再通过已知的红外反光球与手术工具执行端的位置关系，推算出手术工具执行端相对红外摄像头位置关系。