[发明专利]六轴IMU输出数据校准输出空间三维坐标的方法及装置

权利要求书

1.六轴IMU输出数据校准输出空间三维坐标的方法，其特征在于：

其包括以下步骤：

(1)从摄像头采集得到的视频流数据，经过图像预处理、连通域提取得到二维图像的标记点几何中心亚像素坐标，坐标值存入Block RAM，PS端通过AXI Block RAM Controller读取坐标值，视频流数据经过Video in to AXI4-Stream IP核转换成AXI4-Stream IP格式数据流，由VDMA IP核通过AXI Smartconnect总线的HP接口存入DDR3存储器，AXIInterconnect总线实现GP接口与外设配置接口的互联，从而实现PS端控制PL端的外设；

(2)标记点几何中心亚像素坐标计算在PL端实现，假设手术器械上安装有n个标记点，则在左右摄像头拍摄的图像中有相应的n个光点区域，设在第j(j＝1,2…n)个光点区域有m个像素点的灰度值大于指定阈值t，其中第i(i＝1,2…m)个像素点的坐标表示为q_i＝[q_ix,q_iy]^T，并且该像素点的灰度值表示为v_i，则该光点的亚像素坐标q_i由下式得到

(3)标记点亚像素坐标的立体匹配在PS端实现，通过基于极线几何约束的原理完成立体匹配后，通过空间点的三维重建获取标记点在实际空间的三维坐标。

2.根据权利要求1所述的六轴IMU输出数据校准输出空间三维坐标的方法，其特征在于：所述步骤(1)中，配置VDMA IP核的帧缓存空间大小、读写通道以及接收坐标计算完成的信号，通过AXI Interconnect接口在PS端修改PL端IP核中寄存器的值，从而控制图像传感器的曝光模式、曝光时间、近红外LED灯的点亮时间、以及调整连通域提取的阈值。

3.根据权利要求2所述的六轴IMU输出数据校准输出空间三维坐标的方法，其特征在于：所述步骤(2)中，一把手术器械上安装多个标记点，利用灰度阈值t、距离阈值l以及相邻区间m来把每一个大于阈值的像素点划分到相应的光点区域，其中，若找到一个符合条件的点，其邻域m指的就是以该点左上顶点、边长为1×m的矩形区域。

4.根据权利要求3所述的六轴IMU输出数据校准输出空间三维坐标的方法，其特征在于：所述步骤(3)中，设p₁和p₂分别为任意空间点P在左、右摄像机上的图像点，并已求得了二者的像素坐标；假设P的空间三维坐标为(X_W,Y_W,Z_W)，p₁和p₂的像素坐标分别为(u₁,v₁)和(u₂,v₂)，两摄像头的投影矩阵分别为：

计算P的空间三维坐标为(X_W,Y_W,Z_W)的四个线性方程为：

5.根据权利要求4所述的六轴IMU输出数据校准输出空间三维坐标的方法，其特征在于：所述步骤(3)中，公式(4)、(5)分别表示通过O₁p₁、O₂p₂的直线，点P是两者的交点，则同时满足两式，故联立两式求解在世界坐标系下点P的坐标；因为上述两个方程组共包含四项线性方程，共有三个未知数，故三项方程求得(X_W,Y_W,Z_W)的值；实际情况中，噪声的存在影响PS端处理获得的数据，采取最小二乘法求解空间点三维坐标。