一种基于FPGA的内窥镜姿态横滚角的实时解算方法与流程

本发明涉及医疗内窥镜图像处理与惯性姿态测量，尤其涉及一种基于fpga的内窥镜姿态横滚角的实时解算方法。

背景技术：

1、在现代医疗手术内窥镜（如胃镜、肠镜、腹腔镜）及工业管道内窥镜系统中，镜头的姿态实时感知对于图像自动防抖、水平校正以及三维重建至关重要。其中，横滚角（roll）的精确计算直接决定了内窥镜输出图像是否会发生令人眩晕的旋转，是维持操作医生“头顶向上”视觉习惯的核心参数。

2、在现有的内窥镜姿态测量系统中，横滚角的计算主要基于微处理器（如mcu、dsp或arm）通过编写软件程序来实现。该方案通常采用串行执行的软件架构，其核心处理流程如下：首先，微处理器通过i2c或spi总线，利用中断或轮询方式依次读取微型imu（惯性测量单元）传感器的三轴加速度和三轴角速度，完成串行数据采集。然后，在处理器内核中运行c/c++编写的数学运算程序，一方面计算基于加速度的横滚角，另一方面对角速度进行积分。随后，处理器调用数学库中的浮点运算单元，执行卡尔曼滤波或互补滤波算法，将上述两种数据融合，输出最终的横滚角数值，用于驱动后端图像处理器（isp）进行实时图像旋转补偿。所有计算过程均在操作系统的任务调度或主循环下完成，与其他系统任务（如通讯、视频编解码）共享cpu资源。

3、这种基于通用微处理器（软件实现）的方案在高性能内窥镜姿态控制和图像实时校正场景下存在以下显著缺点：

4、延迟抖动导致视频画面撕裂： 由于微处理器采用指令流水线的串行执行方式，且受到中断响应、任务切换和系统调度的影响，从采集传感器数据到输出角度的延迟时间不是完全固定的。在内窥镜快速旋转或抖动时，这种微小的延迟抖动会导致后端图像旋转校正出现相位滞后，引发视频画面撕裂或视觉迟滞，加剧操作医生的眩晕感。

5、计算负载制约采样频率： 在处理高阶滤波算法时，会消耗大量的cpu时钟周期。若要提高imu采样频率以捕捉高频手部震动，处理器的负载会剧增，迫使开发者不得不简化算法，从而牺牲了动态校正精度。

6、多源数据不对齐引入空间误差： 微处理器在读取imu数据时通常需要cpu干预，无法实现真正意义上的并行同步采集。在多传感器融合时，数据到达的时间差会引入额外的对齐误差，降低内窥镜的静态测量准确度。

7、功耗与发热量大： 内窥镜前端探头空间极度受限且对散热要求严苛（医疗级探头表面温升有严格限制）。为了完成复杂的三角函数和乘法，微处理器频繁访问内存，功耗与发热量远高于专用硬件电路，极易导致探头超温。

8、因此，亟需一种能够消除延迟抖动、提高采样吞吐量、极低功耗且无发热隐患的内窥镜姿态横滚角实时解算方法。

技术实现思路

1、本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例，在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

2、鉴于上述现有存在的问题，提出了本发明。

3、为解决上述技术问题，本发明提供如下技术方案：

4、fpga内的数据同步采集模块通过总线接口读取内窥镜探头内加速度计和陀螺仪的原始数据，根据预设量程将原始数据转换为第一定点数格式的加速度值和角速度值。

5、fpga内部的加速度解算模块利用cordic运算ip核计算加速度值对应的第一横滚角，同时角速度积分模块根据上一周期的输出横滚角与当前角速度值和采样周期乘积的和计算第二横滚角，且两个模块以流水线方式并行执行。

6、归一化修正模块计算第一横滚角与第二横滚角的差值，判断差值是否超过180度或小于负180度，若是则对差值进行加或减360度修正，输出最短路径偏差值。

7、互补滤波模块将最短路径偏差值与预设的权重系数相乘，再将乘积与第二横滚角相加，得到最终的内窥镜横滚角并输出至图像处理单元进行视觉水平校正。

8、作为本发明所述的一种优选方案，其中：所述数据同步采集模块采用状态机驱动iic或spi总线，将加速度计数据定点化为q1.14格式，将陀螺仪数据通过移位加/减法链转换为角速度。

9、作为本发明所述的一种优选方案，其中：所述加速度解算模块通过cordic ip核计算arctan2，并利用移位加/减法链实现弧度转角度的乘法运算，将角度范围转换为[0°,360°]的q9.6定点数。

10、作为本发明所述的一种优选方案，其中：所述角速度积分模块在首次计算时将加速度解算模块的输出赋给积分初值，实现零延迟初始对准。

11、作为本发明所述的一种优选方案，其中：所述归一化修正模块通过比较器判定偏差是否超出±180°，并执行加/减360°修正。

12、作为本发明所述的一种优选方案，其中：所述互补滤波模块将滤波公式变形为：

13、

14、其中，为最终输出的横滚角，为角速度积分模块输出的第二横滚角，为互补滤波器中陀螺仪通道的权重系数，为归一化修正模块输出的最短路径偏差值。

15、作为本发明所述的一种优选方案，其中：各模块通过级间寄存器隔离，形成单时钟周期触发的流水线运作。

16、本发明的有益效果：fpga硬件流水线架构消除了软件调度和中断造成的不确定性，解算延迟固定为几个时钟周期，相位滞后稳定，有利于高带宽控制；加速度解算和角速度积分并行执行，采样频率可达数khz而不增加计算负载，吞吐量远高于串行处理器；通过定制化定点数位宽（q1.14、q9.6、q10.6）和移位加/减法链替代乘法器，在同等采样率下功耗相比dsp显著降低，且可在低端fpga上运行；首次计算时强制将加速度角赋给积分初值，消除了互补滤波器的启动爬升过程，实现开机即用；圆周拓扑归一化修正机制确保角度在0°/360°边界处的无突变输出，避免控制环路震荡。

1.一种基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：包括：

2.如权利要求1所述的基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：所述数据同步采集模块采用状态机驱动串行总线接口，将加速度计数据定点化为预设定点格式，将陀螺仪数据通过移位加/减法链转换为角速度。

3.如权利要求1所述的基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：所述加速度解算模块通过cordic ip核计算arctan2，并利用移位加/减法链实现弧度转角度的乘法运算，将角度范围转换为[0°,360°]的q9.6定点数。

4.如权利要求1所述的基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：所述角速度积分模块在首次计算时将加速度解算模块的输出赋给积分初值，实现零延迟初始对准。

5.如权利要求1所述的基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：所述归一化修正模块通过比较器判定偏差是否超出±180°，并执行加/减360°修正。

6.如权利要求1所述的基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：所述互补滤波模块将滤波公式变形为：

7.如权利要求1所述的基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：各模块通过级间寄存器隔离，形成单时钟周期触发的流水线运作。

8.如权利要求1所述的基于fpga的内窥镜姿态横滚角的实时解算方法，其特征在于：所述互补滤波模块输出的最终横滚角实时传输至后端的图像处理单元（isp），所述图像处理单元根据所述最终横滚角对内窥镜采集的视频画面进行实时反向旋转补偿，以实现视觉水平校正。