第２６卷第８期 ２０１３年８月 传感技术学报 ＣＨＩＮＥＳＥ ＪＯＵＲＮＡＬ ＯＦ ＳＥＮＳＯＲＳ ＡＮＤ ＡＣＴＵＡＴＯＲＳ Ｖ０１．２６ ＮＯ．８ Ａｕｇ．２０１３ Ｍｕｌｔｉ・Ｓｅｎｓｏｒ Ａｔｔｉｔｕｄｅ Ｃｏｍｐｕｔａｔｉｏｎ ｆｏｒ Ｅｎｄｏｓｃｏｐｅ ｉｎ Ｍａｇｎｅｔｉｃ Ｉｎｔｅｒｆｅｒｅｎｃｅ Ｅｎｖｉｒｏｎｍｅｎｔ ＷＡＮＧ Ｓｅｎ ，ＣＨＥＮ Ｘｉａｏｄｏｎｇ ， ，ＤＵ Ｃｈｅｎｇｙａｎｇ ，ＷＡＮＧ Ｙｉ ，ＹＵ Ｄａｏｙｉｎ ＇ （１．Ｃｏｌｌｅｇｅ ｏｆ Ｐｒｅｃｉｓｉｏｎ Ｉｎｓｔｒｕｍｅｎｔｓ ａｎｄ＠ｔｏ—Ｅｌｅｃｔｒｏｎｉｃ Ｅｎｇｉｎｅｅｒｉｎｇ，孔ｎｎｊｉｎ Ｕｎｉｖｅｒｓｉｔｙ，Ｔｉａｎｊｉｎ ３０００７２，Ｃｈｉｎａ； ２．Ｋｅｙ Ｌａｂｏｒａｔｏｒｙ ｏｆ＠ｔｏ—Ｅｌｅｃｔｒｏｎｉｃｓ Ｉｎｆｏｒｍａｔｉｏｎ Ｔｅｃｈｎｏｌｏｇｙ，Ｔｉａｎｊｉｎ Ｕｎｉｖｅｒｓｉｔｙ，死ｎｎｊｉｎ ３０００７２，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｉｎ ｃｌｉｎｉｃａｌ ｅｎｄｏｓｃｏｐｙ ｅｘａｍｉｎａｔｉｏｎ，ｄｏｃｔｏｒｓ’ｌｉｍｉｔｅｄ ｐａｔｈｏｌｏｇｙ ｅｘｐｅｒｉｅｎｃｅ ｃａｎ ｌｅａｄ ｔｏ ｈｕｇｅ ｓｕｂｊｅｃｔｉｖｅ ｅｒｒｏｒ ａｎｄ ａｆｆｅｃｔ ｔｈｅ ｆｉｎａｌ ｄｉａｇｎｏｓｉｓ．Ａｌｔｈｏｕｇｈ ＥＭＴ（Ｅｌｅｃｔｒｏ—ｍａｇｎｅｔｉｃ Ｔｒａｃｉｎｇ）ｔｅｃｈｎｏｌｏｇｙ ｃｏｕｌｄ ｂｅ ｅｆｆｅｃｔｉｖｅ ｉｎ ｓｏｌｖｉｎｇ ｔｈｉｓ ｐｒｏｂｌｅｍ ｂｙ ｓｃｉｅｎｔｉｆｉｃａｌｌｙ ｔｒａｃｋｉｎｇ ｔｈｅ ｍｉｃｒｏ ｅｎｄｏｓｃｏｐｉｃ ｐｒｏｂｅ，ｔｈｅ ｐｒｅｃｉｓｉｏｎ ｃｏｕｌｄ ｄｅｃｌｉｎｅ ｂａｄｌｙ ｗｈｅｎ ｉｔ ｉｓ ｄｉｓｒｕｐｔｅｄ ｂｙ ｆｅｒｒｏｍａｇｎｅｔｉｓｍ ｍｅｄｉｃａｌ ｉｎｓｔｒｕｍｅｎｔｓ ａｎｄ ｍａｇｎｅｔｉｃ ｄｉｓｔｏｒｔｉｏｎ ｏｃｃｕｒｓ．Ｉｎ ｔｈｉｓ ａｒｔｉｃｌｅ，ａｎ ｉｍｐｒｏｖｅｄ ａｔｔｉｔｕｄｅ ｃｏｎｖｅｒｇｅｎｃｅ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｍｕｈｉ—ｓｅｎｓｏｒ ｆｕｓｉｏｎ ｔｅｃｈｎｏｌｏｇｙ ｉｓ ｐｒｏｐｏｓｅｄ，ｔｈｅ ｓｙｓｔｅｍ ｃｏｍｂｉｎｅｓ ｉｎｅｒｔｉａｌ ｍｅａｓｕｒｅｍｅｎｔ ｗｉｔｈ ＥＭＴ ｔｅｃｈｎｏｌｏｇｙ，ａｐｐｌｉｅｓ ｑｕａｔｅｍｉｏｎ ｄｉｆｆｅｒｅｎｔｉａｌ ｅｑｕａｔｉｏｎ ａｎｄ ｔｗｏ・ｖｅｃｔｏｒ ｅｒｒｏｒ ｃｏｎｖｅｒｇｅｎｃｅ ｍｏｄｅｌ ｔｏ ｇｅｔ ｔｈｅ ｓｔａｂｌｅ ａｔｔｉｔｕｄｅ ｏｕｔｐｕｔ；Ｔｈｅ ｃｏｎｖｅｒｇｅｎｃｅ ｍｏｄｅｌ ｉｓ ｃｏｎｓｔｒｕｃｔｅｄ ｂｙ ＦＱＡ（Ｆａｃｔｏｒｅｄ Ｑｕａｔｅｒｎｉｏｎ Ａｌｇｏｒｉｔｈｍ） ｍｅｔｈｏｄ ｃｏｎｖｅｒｇｉｎｇ ｔｈｅ ｄｒｉｆｔ ｅｒｒｏｒ ｃａｕｓｅｄ ｂｙ ３－ａｘｉｓ ＭＥＭＳ ｇｙｒｏｓｃｏｐｅ．Ｗｅ ａｌｓｏ ｆｕｓｅ ａ ｔｈｒｅｓｈｏｌｄ ｃｒｉｔｅｒｉｏｎ ｉｎ ｃａｓｅ ｏｆ ｔｈｅ ｈａｒｄ—ｏｖｅｒ ｍａｇｎｅｔｉｃ ｄｉｓｔｏｒｔｉｏｎ ｉｎ ｏｒｄｅｒ ｔｏ ｅｎｓｕｒｅ ｔｈｅ ｏｖｅｒａｌｌ ａｃｃｕｒａｃｙ ａｎｄ ｒｏｂｕｓｔｎｅｓｓ．Ｔｈｅ ｐｒｅｌｉｍｉｎａｒｙ ｅｘｐｅｒｉｍｅｎｔ ｓｈｏｗｓ ｔｈａｔ ｍｕｌｔｉｓｅｎｓｏｒｙ ｓｙｓｔｅｍ ｐｏｓｓｅｓｓｅｓ ｇｏｏｄ ａｃｃｕｒａｃｙ ｗｉｔｈ ａｎｇｕｌａｒ ｄｒｉｔｆ ｌｅｓｓ ｔｈａｎ １。ａｎｄ ｃｏｕｌｄ ｍａｉｎｔａｉｎ ｈｉｇｈ ａｎｔｉ— ｉｎｔｅｒｆｅｒｅｎｃｅ ｓｔａｂｉｌｉｔｙ． Ｋｅｙ ｗｏｒｄｓ：ｅｎｄｏｓｃｏｐｅ；ａｔｔｉｔｕｄｅ ｃｏｍｐｕｔ￣ｉｏｎ；ｍａｇｎｅｔｉｃ ｉｎｔｅｒｆｅｒｅｎｃｅ；ｍｕｌｔｉ－ｓｅｎｓｏｒ；ｉｎｅｒｔｉａｌ ｓｅｎｓｏｒ； ＥＥＡＣＣ：７２３０Ｍ ｄｏｉ：１０．３９６９／ｊ．ｉｓｓｎ．１００４—１６９９．２０１３．０８．０１５ 磁干扰环境下基于多传感器的内窥镜姿态定位技术 王 森 ，一，陈晓冬’ ，杜承阳 ，汪 毅 一，郁道银 （天津大学精密仪器与光电子工程学院，天津３０００７２；天津大学光电信息技术教育部重点实验室，天津３０００７２） 摘 要：在内窥镜临床检查中，医生往往依靠病理经验来判断内窥镜探头在人体内的姿态方位，具有较大的主观误差，影响 病灶的诊断结果。虽然电磁跟踪技术ＥＭＴ（Ｅｌｅｃｔｒｏ—Ｍａｇｎｅｔｉｃ Ｔｒａｃｉｎｇ）可以对内窥镜的探头进行科学定位从而解决上述问题， 但是在铁磁性医疗仪器的干扰下，磁场模型的畸变导致该方法的精度严重下降。提出一种基于多传感器的姿态收敛技术，在 电磁定位的基础上增加ＩＭＵ惯性测量单元，通过四元数微分方程以及双矢量误差收敛模型得到稳定的姿态输出结果；其中， 收敛模型采用分离式收敛的ＦＱＡ算法降低了三轴ＭＥＭＳ磁场传感器的误差发散性，并结合磁干扰阈值判断，保证了在磁干扰 环境下内窥镜姿态解算的精度和鲁棒性，实验表明该系统的姿态定位误差小于１。且具有较强的抗干扰性。 关键词：内窥镜；姿态定位；磁干扰；多传感器；惯性传感； 中图分类号：ＴＰ２１２．９ 文献标识码：Ａ 文章编号：１００４－１６９９（２０１３）０８－１１１０－０６ 目前，内窥镜以及以内窥镜为基础的成像技术 为了提高内窥镜检查中的病灶定位精度和速 （如超声内窥镜）已经成为医生观察病灶组织性状、 早期疾病诊断、提高诊断效率和准确率的主要手段。 度，国内外开展了内窥镜的辅助定位研究，已有的定 位技术主要分为基于透视扫描的图像重建技术¨ 然而，在内窥镜检查中，病灶的定位主要依靠医生通 过内窥镜图像做出的经验性判断，诊断耗时长、存在 主观误判可能，而且需要在前期对医生进行大量的 相关培训。 项目来源：国家“十二五”科技支撑计划项目（２０１２ＢＡＩ１９Ｂ０２） 收稿日期：２０１３—０４—１８ 修改日期：２０１３—０７－２０ 和基于微型传感的磁定位技术 两类。基于透视 扫描的图像重建技术利用透视扫描数据进行图像重 建，即在超声或层析成像的基础上，利用二维图像信 息重构内窥镜及内腔器官的三维信息，这种方法的 第８期 王 森，陈晓冬等：磁干扰环境下基于多传感器的内窥镜姿态定位技术 １１１１ 缺陷是图像处理周期长，实时性差，对人体存在辐射 伤害，且价格昂贵。而基于微型传感的磁定位技术 则是利用微型传感器探头，对医疗仪器进行实时的 姿态测定与跟踪。目前已有的电磁追踪技术ＥＭＴ 通过利用人体外部的永磁体或磁场线圈产生激励磁 场，以磁场分布的数学模型为基准，配合磁场传感器 的探测数据从而计算得到磁传感点所在的矢量方向 和空间位置，这种方法对于磁场分布的准确性要求 较高，而在医疗诊断和手术中，各类强磁金属仪器必 然会影响磁场分布，从而使定位结果产生较大误 差¨６ｊ，特别是针对技术较为先进的超声内镜的跟 踪，由于超声探头的磁干扰，使得基于磁场模型的定 位误差较大＂ ，无法满足实际的定位需求。 本文采用的多传感式的姿态定位技术 ｊ，通 过在内窥镜前端探头处嵌入磁场传感器以及惯性测 量单元，利用四元数微分方程的迭代更新以及多矢 量的姿态收敛来完成姿态的连续解算。 针对姿态追踪过程中的磁干扰，包括超声内窥 镜前端探头或者微创手术中其他铁磁性金属物质的 影响，本文提出了一种收敛阶段的ＦＱＡ（Ｆａｃｔｏｒｅｄ Ｑｕａｔｅｒｎｉｏｎ Ａｌｇｏｒｉｔｈｍ）算法，通过陀螺仪的迭代更新 得到姿态的估计值，在矢量收敛时顺次利用重力矢 量和地磁场矢量进行误差函数的最优估计，将惯性 测量单元作为姿态解算的核心，从而降低磁传感的 不稳定性对姿态解算的影响。 １ 系统结构 如图１所示，为了实现内窥镜多传感器的姿态 定位，内窥镜前端探头的一种理想结构ｌｌＯ］。在电子 内窥镜微型ＣＭＯＳ摄像模组前端结构中，将三轴 ＭＥＭＳ加速度传感器、三轴ＭＥＭＳ陀螺仪以及三轴 ＭＥＭＳ磁场传感器与ＣＭＯＳ驱动电路集成在一起， 并处于同一个载体坐标系Ｏ．ＸＹＺ上，姿态的解算就 是对０一ＸＹＺ坐标系方位角的估计。所有传感器都 并接在一路ｉｚＣ总线上，以实现数据的传输。 ＣＭｏ 图１ 内镜前端多传感定位结构示意图 本系统中，为了减小整个传感结构占用的空间， 传感器模组采用了ＩＭＵ惯性测量单元ＭＰＵ６０５０以及 高磁场灵敏度的ＨＭＣ５８８３Ｌ芯片。ＭＰＵ６０５０为全球 首例整合性６轴运动处理组件，内部包含一个１３１ ＬＳＢｓ／。／ｓｅｃ敏感度与全格感测范围为±２５０。／ｓｅｃ、 ±５００。／ｓｅｃ、±１ ０００。／ｓｅｃ与±２ ０００。／ｓｅｃ的三轴角速 度感测器（陀螺仪）以及一个可程式控制，且程式控 制范围为±２ ｇ ±斗ｇ ±８ ｇ 和＋１６ ｇ 的三轴加速度 传感器，具有４ ｍｍｘ４ ｍｍｘＯ．９ ｍｌｉｌ（ＱＦＮ）的微型包 装，该融合芯片的特征在于不仅免除了组合陀螺仪与 加速器时之轴间差的问题，而且可以通过高达４００ ｋＨｚ的Ｉ ｃ总线传上传数据。ＨＭＣ５８８３Ｌ是一款３．０ ｍｉｎｘ３．０ ｍｍｘＯ．９ ｍｍ ＬＣＣ表面贴装的三轴磁场传感 器，内置１２ ｂｉｔ ＡＤＣ等集成电路，采用霍尼韦尔各向 异性磁阻（ＡＭＲ）技术，能在－＋８ Ｇａｕｓｓ的磁场中实现 ２ ｍＧａｕｓｓ的分辨率，且带置位／复位和偏置驱动器用 于消磁、自测和偏移补偿，其传输通道也是Ｉ ｃ总线。 上述模组中的传感器通过Ｉ Ｃ总线由单片机 ＭＣＵ统一发送指令进行信息采集，本课题应用在内 窥镜检查这种线性运动速度较慢，姿态变换较为频 繁的场合，因此三轴加速度传感器主要负责对重力 加速度矢量在载体坐标系下分布状态的测量，同时 三轴陀螺仪采集坐标系的角速率信息，而三轴磁场 传感器则主要是用来测量大小在４００ ｍＧａｕｓｓ的地 磁场矢量在传感器坐标系上的三轴分量。 实际应用上，在ＣＭＯＳ摄像芯片对病变部位进 行图像采集的同时，探头前端ＸＹＺ坐标系的三轴角 速度和加速度信号、以及地磁场分量的测量数据，通 过ＳＤＡ和ＳＣＬ总线传输到ＣＰＵ进行姿态解算，输 出内窥镜前端在大地坐标系的方位，以实现观测点 的姿态定位。 ２姿态定位主要算法 首先，介绍坐标系及相关符号。Ｃ为坐标系转 换矩阵； 为四元数运算符；［ａ×］为三维向量ａ的 斜对称矩阵；ｏ—ｘｙｚ为大地坐标系ｎ；０－ ｙ＇ｚ 为一次 收敛坐标系ｅ；ｏ— ”为载体实时坐标系ｂ。 ２．１陀螺仪姿态估计 通过三轴ＭＥＭＳ陀螺仪可以实时采集内窥镜 在坐标系Ｏ—ＸＹＺ的三轴瞬态角速度，为了配合姿态 四元数运算，在角速度矩阵中引人一个零元素。 甜 ：［０ ０９ ０３ ０３：］ （１） 姿态四元数是一种对坐标系转动状态的连续表 达方法，在运算中可避免三角运算的复杂性以及奇 异点 ，四元数与坐标系转换矩阵间的关系可由以 １ １ １２ 传感技术学报 ｗｗｗ．ｃｈｉｎａｔｒａｎｓｄｕｃｅｒｓ．Ｃｏｒｎ 第２６卷 下公式来表达。 ｑ：口０＋ｇｏｉ＋ｑｊ＋ｑｏｋ （２） ３－ＤＯＦ（Ｄｅｇｒｅｅ ｏｆ Ｆｒｅｅｄｏｍ）上的扩散，由于该方法采 用直接计算姿态的方式，未对姿态进行收敛调整，精 ｆ－ｑ ＋口 １ｉ１；２（ｑ０ｑ。 ｑ２）２（ｑ －ｑ。ｑ ）］ 度有限，且无法适应磁干扰突变的场合。 ２．２双矢量ＦＱＡ收敛算法 针对陀螺仪随机漂移带来的四元数ｑ的累积误 差，结合式（３），可构造出如下误差能量函数Ｆ（ｑ， ｎ ，ｂ ），其中Ｓ表示空间中的固定矢量，ｎ 和ｂ 分 （ｇ）＝Ｊ ２（窜ｌｇ２－ｑ３口０） ２＿ｇ２１ ２ ；２（ｑ０鼋１ ３ｑ２）ｌ ｌ２（口。ｑ ＋ｇ ｑ３）２（ｑ２ｑ３－－ｑ ｑｏ）鼋 ＿ｑｉ＋口；ｊ Ｉ Ｙ Ｉ＝ｃ：（ｑ）Ｉ Ｙ ｌ （３） 别表示传感器在两坐标系下对矢量Ｓ的实际测量 通过式（３）表示空间中一个固定矢量（重力加 速度或者地磁场）在两坐标系下的坐标值之间的对 应关系。 利用陀螺仪在时间问隔 下进行高速采样，通 过四元数微分方程，在一阶积分下可以连续更新Ｏ— ｘｙｚ的姿态四元数，如下表示。 ｌｒ ｑ ． ，ｚ 了 ，１ ＾ ｆ＿ｌ￣ｔｏ“Ｌ ， Ｉｇ 斗 ： ， ， ＋垡：． △， ＭＥＭＳ陀螺仪在实际工作时的角速度输出带有 一定的随机漂移误差，该有色噪声通过式（４）的不 断迭代，会导致误差随时间逐渐增大，所以通过陀螺 仪的四元数微分方程，仅能在短时间内实现姿态的 估计，如果希望在较长时间内对姿态进行稳定定位， 则需要一些特殊的误差函数对其发散进行收敛。 利用多矢量对姿态进行收敛求解的问题，称为 Ｗａｈｂａ问题ｌ１ ，许多学者研究并提出了多种估计算 法，包括Ｄａｖｅｎｐｏｒｔ提出的ｑ－ｍｅｔｈｏｄ算法、以及之后 改进的ＱＵＥＳＴ算法等，大部分都采用复杂的矩阵运 算，计算量较大。 为了简化运算，目前在数字信号处理领域广泛 采用自适应收敛算法，包括“最速梯度下降”和“高 斯一牛顿”法，它们利用一阶微分Ｊａｃｏｂｉａｎ矩阵，降 低了算法维度，通过迭代完成最优化收敛， Ｂａｃｈｍａｎｎ ＥＲ和Ｊｏａｏ ＬＭ已经将该方法应用于加速 度计和磁强计的信息融合中¨ 。但是这两种算法 要求重力加速度以及地磁场矢量的测量结果具有较 高的精度，因为算法在收敛时需要构建两个矢量的 均方误差模型，来一同对姿态四元数进行收敛ｌＪ 。 实际应用中，ＭＥＭＳ加速度计和磁场传感器的 测量精度差别较大，后者在医疗环境下更容易受到 干扰。为此，Ｙｕｎ Ｘｉａｏｐｉｎｇ等研究人员提出了一种 分离式的姿态解算模型ＦＱＡ（Ｆａｃｔｏｒｅｄ Ｑｕａｔｅｒｎｉｏｎ Ａｌｇｏｒｉｔｈｍ）算法¨ ，这种方法的优点是在进行姿态 解算时可以将磁场传感器所引入的误差限定在一个 固定的轴上，避免磁干扰产生的姿态误差在全空间 值，式（６）作为四元数的收敛目标。 Ｆ（口，ｎｓ，ｂｓ）＝ｃ：（ｑ）ｎｓ－ｂｓ （５） ｍｉｎｑ￣ｎ ｌ ｌＦ（ｑ，ｎｓ，ｂｓ）Ｉ『＝＝＞０ （６） 利用一阶微分Ｊａｃｏｂｉａｎ矩阵，可以得到目标函 数的下降方向，通过选取合适的步长，即可实现对上 述目标函数的最小化逼近。如式（７）、式（８）所示： ＡＦ（ｑ，，ｌｓ，ｂｓ）：Ｊ（ｑ，ｎｓ） Ｆ（ｑ，ｎｓ，ｂｓ） （７） 【 （８） 最速梯度法和高斯牛顿法以矢量测量的理想＿ｉ９＝１４ 量结果为前提，将两个矢量的误差能量函数构造在 一起进行收敛，然而ＭＥＭＳ微型传感器的精度具有 较大差别，在医疗环境下，ＭＥＭＳ磁场传感器的精度 更容易受到影响，因此本文在进行收敛时，利用单一 矢量收敛的特性，采用一种分离式ＦＱＡ收敛，基本 模型如图２所示。 图２ ＦＱＡ分离收敛坐标模型图 如图２所示，利用双矢量进行收敛才能将误差 姿态四元数进行全局收敛，根本原因是因为利用单 一矢量 无法收敛自旋的自由度（矢量分布坐标不 变），需要另一个不同方向的矢量日来进行补充 收敛。 分离式收敛利用以上特性，将收敛方向指定如 下，通过两个矢量分步收敛来完成。 ０一ｘ，ｙｚ＂｛－Ｃｅ ，ｑｎ＝ ０一 Ｙ ＋ｃ ，ｑｅ＝ ０－ （９） 第ｌ步的收敛对矢量测量值精度要求较高，大 第８期 王森，陈晓冬等：磁干扰环境下基于多传感器的内窥镜姿态定位技术 其中 Ａｘ＝，竹ｈ－ｍ ，Ａｘｌ＝Ｊ，ｌ Ｇ 一ｍ Ｇ １１１３ 部分的倾斜自由度均是通过这一步骤完成，所以由 稳定性和精度较高的三轴ＭＥＭＳ加速度传感器结 合式（５）一式（８）得到，此时Ｓ＝Ｇ，利用重力加速度 矢量收敛如下： Ａｙ＝ｍ６ －ｍ ，Ａｙ１＝ｉｌｌ Ｇ 一 Ｇ： Ａｚ＝ｍｋ— ，Ａｚ１：ｍ Ｇｙ－ｍ ｖＧ （１６） ，ｌＧ＝［０ ０ １］ ｂＧ＝［Ｇ Ｇ Ｇ ］ ’ 式（１６）里的变量都可以根据第１步收敛的结果求 出或直接测出，代人式（１５）即可得到第２步旋转收 ｆ＿一２ ２ ２霉３—２ｑ。２ｑ ］（１０） ．，（ｑ，，ｌｃ）＝１ ２口ｌ ２ｇｏ ２ｇ３ ２ｇ２ ｌ Ｌ ０ —４ｑ１ —４ｑ２ ０ ｊ 通过重力矢量的梯度下降法，得到从大地坐标 系到中间坐标系的姿态四元数ｑ 和转换矩阵Ｃ：。 根据单矢量收敛的特性可知，在完成初步收敛 后，围绕重力方向自转的误差需通过地磁场矢量进 行校正和收敛，该旋转角可以利用误差四元数近似 以及旋转四元数模型，直接计算出来。具体推导过 程如下。 第２步收敛时的自转修正角０很小，从中间坐 标系ｅ围绕重力加速度矢量自转为最终的载体坐标 系的旋转误差四元数Ｑ 可以做如下近似： Ｑ ＝［ｃ。ｓ号 ｓｉｎ詈Ｇ ｓｉｎ号 ｓｉｎ詈］ ［ ］ ｑ ＝［Ｇ Ｇ Ｇ ］ ｓｉｎ （１１） 其中Ｇ 、Ｇ Ｇ 表示重力加速度在第１步收敛 结束后各轴上的分量值。参照式（３）可以得到坐标 系ｅ到坐标系ｂ的转换矩阵 的近似表达： ｒ １ ２ｑ ３ —２ｑ ］ （Ｑ ）＝卜２ｑｄ １ ２ｑ Ｊ＝ ３～２［口 ×］ Ｌ ２ｇＰ２ —２ｑ １ １ ｊ （１２） 而三轴ＭＥＭＳ磁场传感器于坐标系 ，ｂ下的 实时测量值分别为ｍ ，ｍ ，构造ｍ 表示在中间坐 标系ｅ下的地磁矢量估计值，其间转换关系如下： ＝ｃｅｍ ６＝Ｃｍ （１３） 由式（１１）、式（１２），结合斜对称矩阵的性质，可推导 出第２步收敛的误差模型，其中ｍ 可根据第１步收 敛的结果预先求出。 ｏ－ｍ＝ｍ６一ｍ ＝－２［ｑ ｘＪｍ ＝２Ｌｍ ￣ｊｑ （１４） 因此，以误差模型 ｍ—÷０为目标，通过式（１１）～ 式（１４）可以构建出误差函数ｆ（ｇＯ）＝Ｊ Ｊ Ｏ＇￣ｒｌ Ｊ Ｊ ，其中 ＝ｓｉｎ ，根据抛物线特征，在误差函数最低点，求 得该自转角的表达为： ａｒｃｓｉｎ 等 敛的自转角。 整体的算法流程图如图３所示，通过这种分离 式的收敛方法，可以在内窥镜的姿态定位中，以惯性 传感导航作为重点，，从而保证在磁干扰环境下，磁 场传感器带来的不稳定性影响在第２步收敛， 即自转角的误差上。 图３姿态估计与ＦＱＡ收敛的算法流程 其中分离式ＦＱＡ收敛算法，需要在每一步的四 元数结果后，对四元数进行归一化，并返回陀螺仪的 迭代过程，同时在是否利用磁强计进行收敛时，加入 阈值判断，根据磁扰动的状况选用惯性传感或三传 感的姿态判定方法。 ３实验结果分析 在具体的实验当中，将ＭＰＵ６０５０＋ＨＭＣ５８８３Ｌ 组成的传感器模组封装在一个直径为８ ｍｍ的玻璃 管内，来模拟内窥镜前端的微型结构，并将该玻璃管 固定在三轴弱磁转台上进行姿态测量，如图４所示。 图４● 微型传感探头实验模型 首先在没有磁场干扰时，对姿态定位的精度进 行实验，将探头固定在转台的ｚ轴上，通过转台转 轴上的电位器来测量所转动的真实角度，与上述算 １１ １４ 传感技术学报 ｗｗｗ．ｃｈｉｎａｔｒａｎｓｄｕｃｅｒｓ．ｓｏｒｅ 第２６卷 法的姿态结果进行对比，误差通过三轴的算法结果 与真实角度值的误差根均方来表示： ＲＭＳ（０。…）＝￣／△ ＋ ＋ＡＴ （１７） 拉近到距离磁传感器５ ｃｍ的近距离，２ Ｓ后撤开干 电池，来观察传统的磁场定位和阈值判断的ＦＱＡ收 敛算法结果之间的区别，如图７所示，其中波动较大 的曲线表示未改进前的ＥＭＴ姿态测量结果。 —————为了能够反应姿态定位结果的稳定性，采用动 态摇摆实验，围绕转台ｚ轴在＋３０。和－＋６０。之间循环 摇摆，旋转角的算法结果以及三轴转角均方根误差 Ｇａｕｓｓ－Ｎｅｗｔｏｎ 一一一一ｄｏｕｂｌｅ－ｃｏｎｖｅｒｇｅ 营 １．５ 的结果如图５所示。 ｔｉｍｅ／ｓ （ａ）Ｍｅａｓｕｒｅｄ ａｎｄ ｅｓｔｉｍａｔｅｄ ａｎｇｌｅ ０ ＆ Ｈ詈ｕ ｏ砬＾ ∞ §＿７ ｔｌｍｅ／ｓ （ｂ）Ｅｒｒｏｒ 图５ 动态摇摆实验下的姿态定位结果 由图５中可知，本文所采用的多传感式姿态定 位的精度稳定在０．５。左右，且能够在动态时保持较 高的稳定性。 为了验证算法具有磁传感器误差发散的作 用，设计了静态下的对比实验，将ＦＱＡ收敛算法同 传统的Ｇａｕｓｓ．Ｎｅｗｔｏｎ算法 进行了对比，保持三轴 转台姿态在（０ ，０。，２５。）下不变，在传感器一侧１０ ｃｍ位置放置一块干电池，由于干电池的影响，导致 磁场传感器对地磁场的测量产生误差，两种算法收 敛的过程和结果如下图６所示。 图６中的较为平滑的收敛曲线是高斯一牛顿法 的收敛结果，这种算法在处理双矢量问题时，以地磁 场矢量同重力加速度的均方误差模型作为标准进行 收敛 Ｊ，虽然收敛后的总能量误差较小（右下），但 是其误差能量是建立在不准确的地磁场测量结果上 的，而且可以从其他几幅图看出，相比经过两步完成 的ＦＱＡ折线收敛模型，地磁场测量的误差扩散到了 各个轴上，与真实角度比较，静态下的双矢量ＦＱＡ 算法可以保持在ＲＭＳ（０…）＜０．８。，而经典高斯一牛 顿在这种干扰环境下的误差发散后，已经超过了 １．５。，实验结果验证了在干扰环境中双矢量ＦＱＡ算 法具有更高的精度。 为了进一步验证算法对于磁干扰的鲁棒性，进 行了突变磁场干扰实验，通过在短时问内将干电池 营ｎｉ ０． ０ ２ ４ ６ ８ Ｔｉｍｅ／ｍｓ Ｔｉｍｅ／ｍｓ （ａ）Ｘ轴定位精度对比 （ｂ）Ｙ轴定位精度对比 ：Ｏ．２０ 。・０５ Ｏ Ｔｉｍｅ／ｍｓ Ｔｉｍｅ／ｍｓ （ｃ）Ｚ轴定位精度对比 （ｄ）收敛速度对比 图６双矢量ＦＱＡ算法与经典Ｇａｕｓｓ—Ｎｅｗｔｏｎ算法对比 图７收敛ＦＱＡ算法对磁干扰的测量结果 通过上图可以看出来，对于８ Ｓ～１０ Ｓ问施加的 铁磁性物质的干扰，纯磁场定位的方法过于敏感，姿 态定位结果瞬间随之突变，而本文所采用的阈值 ＦＱＡ算法，相比ＥＭＴ的电磁姿态解算算法对磁干扰 具有更好的过滤作用，说明多传感的姿态定位技术 更能适应内窥镜所在的医疗磁干扰环境。 ４结论 本文从多传感器姿态定位技术出发研究了一种 分离式ＦＱＡ姿态收敛算法，以惯性传感技术作为姿 态解算的重点，将磁场传感器的收敛在微小自转 角这一自由度上，同时设置磁场异常阈值判断，进一 步增强系统对于铁磁性物质的抗干扰性。系统具有 体积小、成本低、磁干扰稳定性好的特点，在内窥镜等 医疗仪器的电磁导航领域具有广阔的应用前景。 参考文献： ［１］ Ａｂｂａｓｉ Ｈ，Ｃｈｉｎ Ｓ，Ｋｉｍ Ｄ，ｅｔ ａ１．Ｃｏｍｐｕｔｅｒｉｚｅｄ Ｌａｔｅｒａｌ Ｅｎｄｏｓｃｏｐｉｃ Ａｐｐｒｏａｃｈ ｔｏ Ｓｐｉｎａｌ Ｐａｔｈｏｌｏｇｉｅｓ［Ｃ］／／Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｇｒｅｓｓ ；８期 王森，陈晓冬等：磁干扰环境下基于多传感器的内窥镜姿态定位技术 １１１５ Ｓｅｒｉｅｓ，２００１，１２３０：２４０～２４７． 马雪君．基于体绘制的虚拟内窥镜技术研究［Ｄ］．长春理工大 学，２０１０． Ｙａｎｇ Ｗ，Ｈｕ Ｃ，Ｌｉ Ｍ，ｅｔａ１．Ａ Ｎｅｗ Ｔｒａｃｋｉｎｇ Ｓｙｓｔｅｍ ｆｏｒ Ｔｈｒｅｅ Ｍａｇｎｅｔｉｃ Ｏｂｊｅｃｔｉｖｅｓ［Ｊ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｍｅｃｈａｔｒｏｎｉｅｓ， Ｄｅｃｅｍｂｅｒ ２０１０，４６：４０２３—４０２９． 林东，陈新．三维空间磁场结构数学模型及磁场方式三维定位 装置［Ｊ］．福州大学学报（自然科学版），２００３，３１（２）：１５１—１５５． 柳立坤．数字人虚拟定位系统的研究［Ｄ］．天津大学，２０１２． Ｗｕ Ｊ Ｑ，Ｙａｎｇ Ｄ Ｙ，Ｑｉａｎ Ｊ ｗ．Ｃａｌｉｂｒａｔｉｏｎ Ｅｒｒｏｒ Ａｎａｌｙｓｉｓ ａｎｄ Ｗａｙ ｏｆ Ｃａｌｉｂｒａｔｉｏｎ ｆｏｒ Ｓｐａｔｉｌａ Ｃｕｒｖａｔｕｒｅ Ｍｅａｓｕｒｅｍｅｎｔ［Ｊ］．Ｏｐｔｉｃｓ ａｎｄ Ｐｒｅｃｉｓｉｏｎ Ｅｎｇｉｎｅｅｒｉｎｇ，２００３，１ｌ（２）：１６６—１７０． Ｈｕｍｍｅｌ Ｊ，Ｆｉｇｌ Ｍ，Ｋｏｌｌｍａｎｎ Ｃ．Ｅｖａｌｕａｔｉｏｎ ｏｆ ａ Ｍｉｎｉａｔｕｒｅ Ｅｌｅｃｔｒｏ— ｍａｇｎｅｔｉｃ Ｐｏｓｉｔｉｏｎ Ｔｒａｃｋｅｒ［Ｊ］．Ｍｅｄｉｃａｌ Ｐｈｙｓｉｃｓ，２００２，２９（１０）： ２２０５—２２１２． 刘海颖．微小卫星姿态控制系统关键技术研究［Ｄ］．南京航空 航天大学，２００８． 薛亮，李天志．基于ＭＥＭＳ传感器的微型姿态确定系统研究 … … ［Ｊ］．传感技术学报，２００８，２１（３）：４５７—４６０． 王森（１９９０一），男，天津大学光学工 程硕士研究生，主要研究方向是内窥 镜辅助定位，多传感器信息融合，电磁 定位系统等，ｈａｒｒｙｓｅｎ＠１２６．ｃｏｎ； 侯文生，郑小林，彭承琳．体内微型诊疗装置磁定位简化模型 的实验研究［Ｊ］．仪器仪表学报，２００５，２６（９）：８９５—８９７． 杨鹏翔，秦永元．基于欧拉角微分模型的捷联惯导直接非线性 对准方法［Ｊ］．传感技术学报，２０１１，２４（３）：３８６—３９１． Ｗａｈｂａ Ｇ．Ｐｒｏｂｌｅｍ ６５—１：Ａ Ｌｅａｓｔ Ｓｑｕａｒｅｓ Ｅｓｔｉｍａｔｅ ｏｆ Ｓａｔｅｌｌｉｔｅ Ａｔｔｉｔｕｄｅ［Ｍ］．ＳＩＡＭ Ｒｅｖ，Ｊｕｌｙ １９６５，７（３）：４０９． Ｍａｔｉｎｓ Ｊ Ｌ，Ｙｕｎ Ｘ Ｐ，Ｂａｃｈｍａｎｎ Ｅ Ｒ，ｅｔ ａ１．Ａｎ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ ｆｏｒ Ｑｕａｔｅｒｎｉｏｎ—Ｂａｓｅｄ Ｏｒｉｅｎｔａｔｉｏｎ Ｅｓｔｉｍａｔｉｏｎ Ｕｓｉｎｇ ＭＡＲＧ Ｓｅｎｓｏｒｓ［Ｃ］／／Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ２００１ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ，Ｈａｗａｉｉ，ＵＳＡ，２００１： ２００３—２０ｌ１． Ｍａｄｇｗｉｅｋ Ｓｅｂａｓｔｉａｎ Ｏ Ｈ．Ｈａｒｒｉｓｏｎ Ａｎｄｒｅｗ Ｊ Ｌ．Ｅｓｔｉｍａｔｉｏｎ ｏｆ ＩＭＵ ａｎｄ ＭＡＲＧ Ｏｒｉｅｎｔａｔｉｏｎ Ｕｓｉｎｇ ａ Ｇｒａｄｉｅｎｔ Ｄｅｓｃｅｎｔ Ａｌｇｏｒｉｔｈｍ［Ｊ］． ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｅｈａｂｉｌｉｔａｔｉｏｎ Ｒｏｂｏｔｉｃｓ， Ｊｕｌｙ ２０１１． Ｙｕｎ Ｘｉａｏｐｉｎｇ，Ｅｒｉｅ Ｒ Ｂａｃｈｍａｎｎ．Ａ Ｓｉｍｐｌｉｆｉｅｄ Ｑｕａｔｅｒｎｉｏｎ—Ｂａｓｅｄ Ａｌｇｏｒｉｔｈｍ ｆｏｒ Ｏｒｉｅｎｔａｔｉｏｎ Ｅｓｔｉｍ￣ｉｏｎ ｆｒｏｍ Ｅａｒｔｈ Ｇｒａｖｉｔｙ ａｎｄ Ｍａｇｎｅｔｉｃ Ｆｉｌｅｄ Ｍｅａｓｕｒｅｍｅｎｔｓ［Ｊ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｓｔｒｕｍｅｎｔａｔｉｏｎ ａｎｄ Ｍｅａｓｕｒｅｍｅｎｔ，２００８，５７（３）：６３８—６５０． 陈晓冬（１９７５一），男，天津大学光电信 息工程系教授、博导，教育部“新世纪 优秀人才”，中国光学学会高级会员， 主要研究方向为医学内窥成像技术及 仪器、医学图像处理、机器视觉检测 等，ｘｄｃｈｅｎ＠ｔｊｕ．ｅｄｕ．ｅｎ。
