CN101297756A - 磁场与视觉相结合的医疗微型机器人体内姿态定位方法 - Google Patents

Abstract

本发明属于医学工程技术领域，公开了一种分别向三轴正交嵌套的亥姆霍兹线圈装置施加方位角可调整的直流叠加磁场，借助体内微型机器人嵌入无线视觉传输系统与磁检测相结合的途径实现内嵌径向磁化钕铁硼磁体螺旋旋进医疗微型机器人在体内姿态定位的基本方法。其特征是借助机器人嵌入无线视觉系统观察到小磁针随外磁场转动到与机器人轴线重合时，便能将复杂的姿态方位角检测问题转化为外加直流磁场方位角数值可直接读取的已知变量。本发明的有益效果是避免了传统方法需要多个传感器进行检测和复杂的运算过程，以非接触方式快捷的检测出机器人在体内的姿态方位角，为产生相应方位角的旋转磁场提供位姿参数，提高了机器人在弯曲环境内的可操作性。

Description

磁场与视觉相结合的医疗微型机器人体内姿态定位方法

技术领域

本发明属于医学工程技术领域，涉及一种通过摇杆手柄操作带动倾角传感器分别向三轴正交嵌套的亥姆霍兹线圈装置施加以摇杆手柄方位角一致的直流叠加磁场，借助体内微型机器人内嵌磁检测系统与无线视觉传输系统的传输图像相结合的检测方法实现内嵌径向磁化钕铁硼磁体螺旋旋进式医疗微型机器人在体内姿态方位角定位的基本方法。

背景技术

体内医疗微机器人的作业环境是体内的肠道、泌尿系统、血管等，其管状柔弹性壁环境组织内形复杂，蜿蜒曲折，空间狭小，复杂非结构化的流体环境给微机器人的主动控制和操作带来了很大难度。为了不对柔弹性软组织造成创伤和进入体内深处，要求微机器人以无电缆驱动方式，通过游动可靠的进入和退出体内深处，并能安全精细地实现诊断、介入治疗等作业。体内医疗机器人无缆驱动技术对提高人类寿命与生活质量，避免外部手术对人体造成创伤甚至致残具有重要的科学意义，能减轻患者痛苦，缩短康复时间，降低医疗费用，必将对医学工程的发展产生极大的影响。

无缆驱动方式可显著提高体内机器人在复杂环境内的通过性、安全性与可靠性，因此微机器人无缆驱动已经成为国际上的主要发展趋势。日本K.Ishiyama等人提出了利用三轴亥姆霍兹线圈提供空间旋转磁场，作用于胶囊内嵌径向磁化钕铁硼磁体，在胶囊表面螺纹作用下旋进驱动的技术方案，但对如何产生空间旋转磁场并没有介绍，也没有提出机器人的姿态的检测方法。该驱动方法要求外旋转磁场的旋转轴线与机器人的轴线重合，才能有效的驱动机器人旋进行走，因此在弯曲环境内，要求外旋转磁场的轴线可以任意调整，以保证在弯曲柔弹性管内外旋转磁场的旋转轴线始终与机器人的轴线一致，才能实现体内弯曲环境内的驱动。

人体软组织环境蜿蜒曲折，为了适应弯曲环境内的驱动，必须要根据弯曲环境的具体情况，检测出机器人在体内的姿态方位角，并将方位角参数提供给旋转磁场驱动装置，以便产生与机器人姿态方位角一致的外旋转磁场。通过不同弯曲阶段机器人姿态方位角的检测，不断的调整外旋转磁场的轴线位姿，使万向旋转磁场的旋转轴线始终与机器人的轴线方向保持一致，实现体内复杂弯曲环境内的驱动。因此为了实用化，简便、易操作的体内机器人姿态定位技术是必须要解决的关键问题，是实现体内弯曲复杂环境内驱动的必要前提条件。

国外进行了胶囊式机器人相关定位技术的研究，主要针对在体内三维空间位置的定位方法进行了研究，如以色列的胶囊内窥镜系统PillCam采用基于射频信号能量检测的方法进行定位；瑞士和德国的一些研究机构研究了基于永久磁铁磁标记物的方法，这些方法可提供体内目标的大致定位信息，但在定位精度上还存在较大缺陷，尤其是必须借助昂贵的检测设备，操作过程复杂，计算难度大，距离满足临床诊断和治疗要求较远。

国内上海交通大学提出了基于电磁检测原理的定位方法，该方法在体表布置多个电磁发射线圈，通过胶囊内密封的接收线圈检测所在点的磁感应强度，利用检测值，根据磁感应强度和该点位置坐标之间的函数模型反向求解出位置信息。该方法的缺点是要施加交变磁场；同时上海交通大学提出了一种基于体外静磁场检测的胶囊在体内位置连续跟踪的方法，利用胶囊内密封的永久磁铁作磁标记物，在体表布置6个磁场传感器，检测每一采样时刻体表处的磁感应强度，建立磁源和空间磁场分布的对应关系，利用遗传算法求解出胶囊每一时刻的空间位置坐标。上述方法原理复杂，计算量大，可操作性差，尤其是不能检测出机器人的方位角姿态参数。

由于施加的外磁场在人体环境内是均匀的，为了驱动机器人，我们只需要检测出机器人的姿态角即可。针对胶囊机器人在体内方向角姿态检测方法的文献较少，且难度更大。福州大学提出了以磁场方式来测定内窥镜探头在人体内的三维位置及姿态角。根据三个互相正交的圆环线圈在其周围空间任意点产生的三维磁感应强度表达式，以及附着于内窥镜探头上的三个相互正交的磁场传感器在该磁场空间任意点以任意姿态感应磁场时所获得信号的表达式，建立以空间位置与姿态角为未知数的六元非线性方程组，计算出位置与方位角，显然该方法结构过于复杂，计算难度大，可操作性差。

在所申请的发明专利“体内医疗微型机器人万向旋转磁场的驱动控制方法”(申请号：200810011110.2)中，申请人提出一种分别向三轴正交嵌套的亥姆霍兹线圈装置施加以机器人轴线方位角为输入变量的相关幅值和相位的同频率正弦谐波驱动电流，在三轴相互正交嵌套的亥姆霍兹线圈装置包围的一定空间内叠加形成旋转轴线与机器人轴线一致的万向旋转磁场，通过方位角的改变，可实现内嵌径向磁化钕铁硼磁体螺旋旋进医疗微型机器人沿体内任意方向驱动行走的基本方法。但是该方法没有解决如何检测机器人轴线方位角的检测问题，因此本项专利申请又提出机器人轴线方位角的检测技术途径，并将方位角参量输入到旋转磁场驱动系统，便产生了以机器人轴线相垂直的旋转磁场，驱动机器人在肠道内的行走。

综上所述，为了实现人体肠道内弯曲环境内的驱动，检测出机器人的姿态的方位角参数技术是体内胶囊机器人实用化的关键。目前，尚未有简便易行的姿态检测方法，该技术实用前景广阔，一旦取得突破，可通过摇杆手柄控制不断检测出机器人在肠道内不同弯曲环境内的方位角姿态参数，为万向旋转磁场驱动装置提供旋转姿态方位角参量，驱动内嵌径向磁化钕铁硼磁体的体内微型螺旋驱动机器人实现弯曲环境内的驱动，大大缩短体内遍历检查的时间，使基于旋转磁场主动控制的体内无创窥视诊断、介入治疗的梦想变成现实。

发明内容

本发明要解决的技术问题是提供一种通过对外加直流磁场的检测与机器人内嵌无线视觉系统标定相结合的体内医疗微型机器人姿态的非接触式在线检测方法与具体操作途径，为万向旋转磁场驱动装置提供方位角姿态参数，以实现内嵌钕铁硼内驱动器螺旋无损伤医疗微型机器人在体内弯曲复杂环境内的全方位驱动行走。

本发明的技术方案是：

将三组轴线相互正交的亥姆霍兹线圈嵌套在一起，构成三轴正交嵌套亥姆霍兹线圈磁场叠加装置，其空间区域包围病床与患者a躯干部分，由摇杆手柄e带动倾角传感器f将检测出的摇杆手柄e的三维姿态方向角参数输入到姿态检测系统控制器g，通过外磁场驱动器h驱动三轴正交嵌套亥姆霍兹线圈磁场叠加装置c产生与摇杆手柄e同方位角的在一定范围内均匀的直流叠加磁场，当调整摇杆手柄e的方向，通过微机器人内嵌检测系统d检测到施加磁场方向与机器人轴线重合时，将摇杆手柄e的姿态参数输入到姿态检测系统控制器g，经分析计算后得出机器人轴线姿态的三维方向角参数，并输入到外磁场驱动制器h分别向三轴亥姆霍兹线圈c组施加与机器人运动方位角相关幅值和相位的同频率正弦谐波驱动电流，由三轴正交亥姆霍兹线圈c叠加为一定区域内的旋转轴线与机器人轴线方向一致的均匀万向旋转磁场，实现医疗微型机器人在体内弯曲环境内的驱动控制。

下面结合附图，说明借助三轴正交亥姆霍兹线圈施加静态磁场，实现机器人姿态检测的原理。

如图2(a)所示，胶囊机器人外部由前透明端盖1，附着螺旋肋的本体外壳2与后端盖3组成，前透明端盖1是为了不遮掩无线视觉系统，附着螺旋肋的本体外壳2是为了旋转时产生推力，后端盖3起密封作用。

如图1所示，与三轴正交嵌套亥姆霍兹线圈磁场叠加装置(c)固定的坐标系为OXYZ，如图2(b)所示为与机器人固结的动坐标系为

由于外加磁场是均匀的，在小磁针6的中心位置作为动坐标系的原点，并认为内驱动器4在小磁针6附近产生的磁场也是均匀的，根据矢量运算的可平移性，使两个坐标系的原点O与o点重合，使动坐标系

的

轴的方向指向钕铁硼内驱动器4的N极，机器人的轴线与z轴固结，N、S极的分界面与yoz平面重合。z轴在固定坐标系为OXYZ中的方位角α、β、γ，即为要求解的机器人姿态。

动坐标系

可视为由固定坐标系OXYZ按如下坐标旋转变换得到，坐标系的旋转过程如图3所示，首先绕X轴顺时针旋转θ₁角得到一个中间坐标系；然后绕中间坐标系的Y轴旋转θ₂角，此时Z轴与机器人轴线z重合，得到了机器人轴线的方位角；再绕Z轴旋转角度φ，内驱动器4的N极设定在坐标系

轴的正方向，旋转角度φ体现了内驱动器磁极姿态对外施加磁场的影响。由图2可得θ₁、θ₂、α、β、γ之间的关系如下：

${\sin \mathrm{\θ}}_1=\frac{\cos \mathrm{\β}}{\sin \mathrm{\α}},$ ${\cos \mathrm{\θ}}_1=\frac{\cos \mathrm{\γ}}{\sin \mathrm{\α}},$ sinθ₂＝cosα，cosθ₂＝sinα

可见θ₁、θ₂可以用α、β、γ表示。经过三轴旋转后的固定坐标系OXYZ和动坐标系

的关系如图4所示。

在固定坐标系OXYZ内，摇摆摇杆手柄e，借助倾角传感器f，调整外加磁场矢量的方位角，当小磁针6旋转到跟机器人轴线重合时，如果机器人本体不载有内嵌径向磁化钕铁硼驱动器4，则此时摇杆手柄e的倾角即为外加磁场的方位角，也就是机器人轴线的姿态方位角，通过三轴正交嵌套亥姆霍兹线圈磁场叠加装置c施加的磁感应强度为 $B=\left[\begin{array}{c}{B}_X\\ B_Y\\ B_Z\end{array}\right]=K\left[\begin{array}{c}\cos \mathrm{\α}\\ \cos \mathrm{\β}\\ \cos \mathrm{\γ}\end{array}\right],$ K为已知系数，在固定坐标系OXYZ内的方向角为 $\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\\ \mathrm{\γ}\end{array}\right],$ 就是机器人轴线的方位角，并有：

$\left\{\begin{array}{c}\frac{B_X}{\sqrt{B_X^2+B_Y^2+B_Z^2}}=\cos \mathrm{\α}\\ \frac{B_Y}{\sqrt{B_X^2+B_Y^2+B_Z^2}}=\cos \mathrm{\β}\\ \frac{B_Z}{\sqrt{B_X^2+B_Y^2+B_Z^2}}=\cos \mathrm{\γ}\end{array}\right.{\cos }^2\mathrm{\α}+{\cos }^2\mathrm{\β}+{\cos }^2\mathrm{\γ}=1---\left(1\right)$

在固定坐标系OXYZ内，通过调节摇杆手柄e，改变外加磁场矢量的方位角，当小磁针6旋转到跟机器人轴线重合时，如果机器人本体载有内嵌径向磁化钕铁硼驱动器4，则外加磁场的方位角不是机器人轴线的姿态角，要通过计算修正求得机器人轴线的姿态方位角，此时外加磁场使小磁针6旋转到跟机器人轴线重合时，三轴正交嵌套亥姆霍兹线圈磁场叠加装置c施加的磁感应强度矢量为： $B^{\′}=\left[\begin{array}{c}{B}_X^{\′}\\ B_Y^{\′}\\ B_Z^{\′}\end{array}\right]=K\left[\begin{array}{c}{\cos \mathrm{\α}}^{\′}\\ {\cos \mathrm{\β}}^{\′}\\ {\cos \mathrm{\γ}}^{\′}\end{array}\right],$ 外施加磁场方向角为 $\left[\begin{array}{c}{\mathrm{\α}}^{\′}\\ {\mathrm{\β}}^{\′}\\ {\mathrm{\γ}}^{\′}\end{array}\right].$

经过上述旋转坐标变换，则在固定坐标系OXYZ内的磁矢量B′在与机器人固定的动坐标系

内的磁矢量表达关系式为：

$\left[\begin{array}{c}{B}_x^{\′}\\ B_y^{\′}\\ B_z^{\′}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\φ}& -\sin \mathrm{\φ}& 0\\ \sin \mathrm{\φ}& \cos \mathrm{\φ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_2& 0& {\sin \mathrm{\θ}}_2\\ 0& 1& 0\\ {-\sin \mathrm{\θ}}_2& 0& \cos {\mathrm{\θ}}_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\cos \mathrm{\θ}}_1& {-\sin \mathrm{\θ}}_1\\ 0& {\sin \mathrm{\θ}}_1& {\cos \mathrm{\θ}}_1\end{array}\right]\left[\begin{array}{c}{B}_X^{\′}\\ B_Y^{\′}\\ B_Z^{\′}\end{array}\right]$

由图4分析可见，矢量B′在动坐标系

沿z轴的投影为矢量B_z′；在动坐标系沿

轴的投影为矢量B_x′，去克服内驱动器4在小磁针6处产生的静磁场b_x，这是外加磁场方位角需要修正的原因；沿y轴的投影为0，因此在动坐标系

内，磁矢量B′、b_x、B_z′在同一平面内，且有 $B^{\′2}=b_x^2+B_z^{\′2},$ 经分析可知，矢量B′在动坐标系三个轴上的分量有如下关系：

$\left\{\begin{array}{c}{B}_x^{\′}=b_x\\ B_y^{\′}=0\\ B_z^{\′}=\sqrt{B^{\′2}-b_x^2}\end{array}\right.---\left(2\right)$

b_x、B′均为为已知量，式(2)即为：

$\left[\begin{array}{c}{b}_x\\ 0\\ B_z^{\′}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\φ}& -\sin \mathrm{\φ}& 0\\ \sin \mathrm{\φ}& \cos \mathrm{\φ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_2& 0& {\sin \mathrm{\θ}}_2\\ 0& 1& 0\\ {-\sin \mathrm{\θ}}_2& 0& \cos {\mathrm{\θ}}_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\cos \mathrm{\θ}}_1& {-\sin \mathrm{\θ}}_1\\ 0& {\sin \mathrm{\θ}}_1& {\cos \mathrm{\θ}}_1\end{array}\right]\left[\begin{array}{c}{B}_X^{\′}\\ B_Y^{\′}\\ B_Z^{\′}\end{array}\right]$

旋转变换矩阵为：

$\left[\begin{array}{ccc}\cos \mathrm{\φ}& -\sin \mathrm{\φ}& 0\\ \sin \mathrm{\φ}& \cos \mathrm{\φ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\cos \mathrm{\θ}}_2& {\sin \mathrm{\θ}}_1{\sin \mathrm{\θ}}_2& \cos {\mathrm{\θ}}_1{\sin \mathrm{\θ}}_2\\ 0& {\cos \mathrm{\θ}}_1& -\sin {\mathrm{\θ}}_1\\ -\sin {\mathrm{\θ}}_2& \sin {\mathrm{\θ}}_1{\cos \mathrm{\θ}}_2& \cos {\mathrm{\θ}}_1{\cos \mathrm{\θ}}_2\end{array}\right]$

$=\left[\begin{array}{ccc}\cos \mathrm{\φ}{\cos \mathrm{\θ}}_2& \cos \mathrm{\φ}{\sin \mathrm{\θ}}_1{\sin \mathrm{\θ}}_2-\sin \mathrm{\φ}\cos {\mathrm{\θ}}_{\text{1}}& \cos {\mathrm{\φ}\cos \mathrm{\θ}}_1\sin {\mathrm{\θ}}_2+\sin \mathrm{\φ}{\sin \mathrm{\θ}}_1\\ \sin \mathrm{\φ}{\cos \mathrm{\θ}}_2& \sin {\mathrm{\φ}\sin \mathrm{\θ}}_1{\sin \mathrm{\θ}}_2+\cos {\mathrm{\φ}\cos \mathrm{\θ}}_1& \sin \mathrm{\φ}{\cos \mathrm{\θ}}_1\sin {\mathrm{\θ}}_2-\cos \mathrm{\φ}{\sin \mathrm{\θ}}_1\\ -{\sin \mathrm{\θ}}_2& \sin {\mathrm{\θ}}_1{\cos \mathrm{\θ}}_2& {\cos \mathrm{\θ}}_1\cos {\mathrm{\θ}}_2\end{array}\right]$

$=\left[\begin{array}{ccc}\cos \mathrm{\φ}\sin \mathrm{\α}& \cos \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\β}}{\sin \mathrm{\α}}-\sin \mathrm{\φ}\frac{\cos \mathrm{\γ}}{\sin \mathrm{\α}}& \cos \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\γ}}{\sin \mathrm{\α}}+\sin \mathrm{\φ}\frac{\cos \mathrm{\β}}{\sin \mathrm{\α}}\\ \sin \mathrm{\φ}\sin \mathrm{\α}& \sin \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\β}}{\sin \mathrm{\α}}+\cos \mathrm{\φ}\frac{\cos \mathrm{\γ}}{\sin \mathrm{\α}}& \sin \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\γ}}{\sin \mathrm{\α}}-\cos \mathrm{\φ}\frac{\cos \mathrm{\β}}{\sin \mathrm{\α}}\\ -\cos \mathrm{\α}& \cos \mathrm{\β}& \cos \mathrm{\γ}\end{array}\right]$

式(2)又为：

$\left\{\begin{array}{c}{b}_x=\cos \mathrm{\φ}\sin \mathrm{\α}{B}_X^{\′}+(\cos \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\β}}{\sin \mathrm{\α}}-\sin \mathrm{\φ}\frac{\cos \mathrm{\γ}}{\sin \mathrm{\α}})B_Y^{\′}+(\cos \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\γ}}{\sin \mathrm{\α}}+\sin \mathrm{\φ}\frac{\cos \mathrm{\β}}{\sin \mathrm{\α}})B_Z^{\′}\\ 0=\sin \mathrm{\φ}\sin \mathrm{\α}{B}_X^{\′}+(\sin \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\β}}{\sin \mathrm{\α}}+\cos \mathrm{\φ}\frac{\cos \mathrm{\γ}}{\sin \mathrm{\α}})B_Y^{\′}+(\sin \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\γ}}{\sin \mathrm{\α}}-\cos \mathrm{\φ}\frac{\cos \mathrm{\β}}{\sin \mathrm{\α}})B_Z^{\′}\\ B_z^{\′}=-\cos \mathrm{\α}{B}_X^{\′}+\cos \mathrm{\β}{B}_Y^{\′}+\cos \mathrm{\γ}{B}_Z^{\′}\\ {\cos }^2\mathrm{\α}+{\cos }^2\mathrm{\β}+{\cos }^2\mathrm{\γ}=1\end{array}\right.---\left(3\right)$

(3)式可求得α、β、γ、φ四个参量，确定机器人的姿态角α、β、γ。即通过三轴正交嵌套亥姆霍兹线圈磁场叠加装置c施加的磁感应强度矢量为： $B^{\′}=\left[\begin{array}{c}{B}_X^{\′}\\ B_Y^{\′}\\ B_Z^{\′}\end{array}\right]=K\left[\begin{array}{c}\cos {\mathrm{\α}}^{\′}\\ \cos {\mathrm{\β}}^{\′}\\ \cos {\mathrm{\γ}}^{\′}\end{array}\right],$ 当小磁针6与机器人轴心标记7重合时，可通过式(3)的修正关系式计算出机器人的实际轴线方向角 $\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\\ \mathrm{\γ}\end{array}\right].$

本发明的效果和益处是：

通过摇杆手柄的调整，实现小磁针磁检测装置与外加磁场方向的随动，借助机器人嵌入无线视觉系统观察到与外磁场随动小磁针与机器人轴线重合时，便能将复杂的姿态方位角检测问题转化为可直接读取的外加直流磁场的方位角变量，这种外加磁场与视觉相结合的姿态检测方法，避免了传统方法需要多个传感器进行检测与复杂的运算过程，以非接触方式快捷、简便的检测出机器人在体内的姿态方位角，操作者通过摇杆手柄的实时控制，可检测出机器人在弯曲环境内不同位置时方位角姿态，为三轴正交嵌套亥姆霍兹线圈磁场叠加装置产生相应方位角的旋转磁场提供位姿参数，实现医疗微型机器人在弯曲环境内的无缆驱动，提高了体内机器人控制的可靠性与实用性。

附图说明

附图1是本发明的机器人姿态检测与外旋转磁场发生装置的结构示意图。

附图2(a)是微机器人及内嵌检测系统的外部结构局部放大图。

附图2(b)是微机器人及内嵌检测系统的内嵌检测系统局部放大图。

附图3是与固定坐标系OXYZ绕X轴旋转θ₁、绕Y轴旋转θ₂得到动坐标系

位置的旋转变换过程图。

附图4是经过三轴旋转后的固定坐标系OXYZ和动坐标系

的转换关系与矢量投影关系图。

附图5是体内微型机器人方位角姿态检测系统的结构框图。

图中：  a患者；b病床；c三轴正交嵌套亥姆霍兹线圈磁场叠加装置；

        d微机器人及内嵌检测系统；e摇杆手柄；f倾角传感器；

        g姿态检测系统控制器；h外磁场驱动器；

        1前透明端盖；2附着螺旋肋的本体外壳；3后端盖；

        4径向磁化钕铁硼内驱动器；5微摄像头和无线视频传送装置；

        6万向小磁针；7轴心标记。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施例。

实施例：

结合附图5，机器人方位角姿态检测系统由摇杆姿态检测装置(包括摇杆手柄e和倾角传感器f)、姿态检测系统控制器g、外磁场驱动器h、机器人内嵌检测系统d三轴正交嵌套亥姆霍兹线圈磁场叠加装置c五部分构成。机器人内嵌检测系统d包括微摄像头和无线视频传送装置5、万向小磁针6和轴心标记7。

具体步骤如下：

1.规划三轴正交嵌套亥姆霍兹线圈组围成的空间，以便包容人体与病床；将三组亥姆霍兹线圈组从大到小的顺序相互正交嵌套的安装方案，完成匹配设计。使三组亥姆霍兹线圈在施加同一直流电流时，在三轴正交嵌套亥姆霍兹线圈组围成的边长为最大方形亥姆霍兹线圈边长三分之二的中心立体区域内产生的静态磁场强度相等。

2.摇杆姿态检测装置的结构主要包括摇杆手柄e和倾角传感器f，摇杆手柄e可以在手动操纵下在三维空间各方向灵活摇摆，带动倾角传感器f(MTi型倾角传感器)输出姿态，通过RS-232/RS422接口实现与姿态检测系统控制器g之间的数据传送，同时可在倾角检测系统程序软件界面中显示出传感器安装平面的空间三轴倾角角度值。

3.姿态检测系统控制器g接收到摇杆姿态检测装置的倾角传感器采集到的空间三轴倾角数据后，将三个倾角数据输入到直流驱动电流参数计算程序模块中，算出对应的三轴正交嵌套亥姆霍兹线圈磁场叠加装置c三轴线圈分别应加载的三路直流电流信号参数，并将所得电流参数传送至外磁场驱动器h。驱动三轴正交嵌套亥姆霍兹线圈磁场叠加装置c叠加后形成一定范围的均匀静态电磁场，且磁场方向和摇杆轴线方向一致。

4.外磁场驱动器h主要由信号发生器和滤波放大器构成。信号发生器是核心，包括单片机和DDS芯片及其外围接口电路。单片机接受上位机姿态检测系统控制器g发送的电流参数，控制DDS芯片产生相应的三路直流电流信号或正弦波交流电流信号。信号发生器输出的三路电流信号经滤波放大器处理后，分别输入三组负载电磁线圈中，驱动线圈产生电磁场。磁场驱动器根据接收到的电流参数，向三轴亥姆霍兹线圈上分别加载相应的直流电流，从而在微机器人工作环境内产生均匀的静态空间磁场。

5.结合附图2(b)，机器人内嵌检测系统d包括微摄像头和无线视频传送装置5和万向小磁针6。微摄像头和无线视频传送装置5由CMOS图像视频采集传感器、及无线传输电路组成，图像模块包括球形玻璃罩、光源、针孔镜头、CMOS图像传感器、无线视频传送装置5。光源由周向均匀分布的四个高亮度白光LED组成。CMOS图像传感器支持PAL制式视频输出，可直接与无线发射器连接。体外接收器采用无线视频接收器，通过视频线连接到计算机上的视频采集卡，以保存、回放视频信息。

万向小磁针6安装在万向旋转支架上，重心在各支架的中心处，因此消除了重力的影响。根据指南针的原理，万向小磁针6在受到外加磁场作用时将指向磁场方向，外加磁场强度较大，因此可以忽略地磁影响，万向旋转架可实现小磁针空间任意方向的摆动。机器人微型摄像头通过内嵌无线视频传输系统在姿态检测系统控制器g显示屏上实时显示小磁针的偏转姿态。不断调整手柄的空间姿态角，观查到万向小磁针6在外磁场作用下与摇杆手柄e形成随动，直到摇杆手柄e运动直到万向小磁针6偏转至与机器人内部一特定标记7重合，此时万向小磁针6与微机器人轴线方向重合。由姿态检测系统控制器g根据公式(3)中的磁场耦合修正程序模块来完成方位角的修正计算，经修正后的三轴倾角即为与当前机器人轴线姿态方位角。姿态检测系统控制器g从摇杆控制器中接收倾角传感器采集到的空间三轴倾角数据，并将这些动态检测数据在姿态测量软件视窗中实时的显示出来，操作者可以从显示屏上直观的看到当前摇杆轴线的空间姿态角。

6.将检测到的方位角经姿态检测系统控制器g旋转磁场运算程序模块计算得出三路交流电流信号的幅值、相位参数，并将所得电流参数传送至外磁场驱动器h，由三轴正交嵌套亥姆霍兹线圈磁场叠加装置c叠加产生相应方位角轴线的空间旋转磁场。

Claims (1)

1、磁场与视觉相结合的医疗微型机器人体内姿态定位方法，其特征在于：

(1)规划三轴正交嵌套亥姆霍兹线圈组围成的空间，以便包容人体与病床；将三组亥姆霍兹线圈组从大到小的顺序相互正交嵌套的安装方案，完成匹配设计，使三组亥姆霍兹线圈施加同一直流电流时，在三轴正交嵌套亥姆霍兹线圈组围成的边长为最大方形亥姆霍兹线圈边长三分之二的中心立体区域内产生的静态磁场强度相等；

(2)通过摇杆手柄(e)的倾角传感器(f)采集到的空间三轴倾角数据去驱动三轴正交嵌套亥姆霍兹线圈磁场叠加装置(c)，在其中心区域叠加形成的均匀直流磁场的方位角与摇杆手柄(e)轴线方向角一致；

(3)借助机器人内嵌检测系统(d)的无线视频传送与姿态检测系统控制器(g)图像接收系统观察到万向小磁针(6)与机器人内部特定标记(7)重合时，即实现了外磁场方位角与机器人轴线重合，将姿态方位角检测问题转化为外加直流磁场方位角数值直接读取的已知变量；

(4)调节摇杆手柄(e)，改变外加磁场矢量的方位角，当小磁针(6)旋转到跟机器人轴线重合时，如果机器人本体载有内嵌径向磁化钕铁硼驱动器(4)，则机器人轴线的姿态角要通过公式(3)修正求得机器人轴线的姿态方位角α、β、γ；

$\left\{\begin{array}{c}{b}_x=\cos \mathrm{\φ}\sin \mathrm{\α}{B}_X^{\′}+(\cos \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\β}}{\sin \mathrm{\α}}-\sin \mathrm{\φ}\frac{\cos \mathrm{\γ}}{\sin \mathrm{\α}})B_Y^{\′}+(\cos \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\γ}}{\sin \mathrm{\α}}+\sin \mathrm{\φ}\frac{\cos \mathrm{\β}}{\sin \mathrm{\α}})B_Z^{\′}\\ 0=\sin \mathrm{\φ}\sin \mathrm{\α}{B}_X^{\′}+(\sin \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\β}}{\sin \mathrm{\α}}+\cos \mathrm{\φ}\frac{\cos \mathrm{\γ}}{\sin \mathrm{\α}})B_Y^{\′}+(\sin \mathrm{\φ}\frac{\cos \mathrm{\α}\cos \mathrm{\γ}}{\sin \mathrm{\α}}-\cos \mathrm{\φ}\frac{\cos \mathrm{\β}}{\sin \mathrm{\α}})B_Z^{\′}\\ B_z^{\′}=-\cos \mathrm{\α}{B}_X^{\′}+\cos \mathrm{\β}{B}_Y^{\′}+\cos \mathrm{\γ}{B}_Z^{\′}\\ {\cos }^2\mathrm{\α}+\mathrm{co}{s}^2\mathrm{\β}+{\cos }^2\mathrm{\γ}=1\end{array}\right.---\left(3\right)$

式中：B′_X、B′_Y、B′_Z为外加磁矢量B′在固定坐标系OXYZ的X、Y、Z三个轴上的投影，为已知量，

$B^{\′}=\left[\begin{array}{c}{B}_X^{\′}\\ B_Y^{\′}\\ B_Z^{\′}\end{array}\right]=K\left[\begin{array}{c}{\cos \mathrm{\α}}^{\′}\\ \cos {\mathrm{\β}}^{\′}\\ \cos {\mathrm{\γ}}^{\′}\end{array}\right]$ 外施加磁场方向角为 $\left[\begin{array}{c}{\mathrm{\α}}^{\′}\\ {\mathrm{\β}}^{\′}\\ {\mathrm{\γ}}^{\′}\end{array}\right]$

b_x为内驱动器(4)在小磁针(6)处产生的大小已知的静磁场，α、β、γ为机器人轴线在固定坐标系OXYZ内的待求姿态方位角，φ为坐标变换时动坐标系oxnyz绕Z轴的待求旋转角。

Images