【技術領域】

本發明涉及三維定位技術，尤其涉及無線膠囊內窺鏡三維位置和三維姿態的定位系統及其定位方法。

背景技術：

當前，臨床的無線膠囊內窺鏡缺點之一就是不能主動受控，要實現這一主動受控功能并方便臨床使用，無線膠囊內窺鏡的三維位置和三維姿態信息要實時反饋給控制系統；同時，無線膠囊內窺鏡在重建的消化道里視覺導航也需要三維位置和三維姿態信息，醫生才能方便操控無線膠囊窺鏡。

針對以上問題，現有技術中普遍利用x光成像、ct(計算機斷層掃描)和mri(核磁共振)成像技術進行三維重建來定位無線膠囊內窺鏡，這種由重建技術間接定位的速度和精度都受到影響，并且射線存在損害，不宜長時間定位。

以色列的givenimaging公司最早提出了一種應用于無線膠囊內窺鏡定位的無線射頻(rf)信號定位技術。其利用在人體外的8個無線射頻天線接收體內無線膠囊內窺鏡發射的無線射頻信號，并利用算法獲取無線膠囊內窺鏡的位置。該方法直接利用了無線膠囊內窺鏡傳輸圖象的無線射頻信號，缺點是定位精度低，平均定位精度為37.7毫米，臨床應用效果不好。

也有人提出了利用永磁定位技術定位無線膠囊內窺鏡。在無線膠囊內窺鏡內部放置永磁體作為磁標記，在人體周圍布置多個磁場傳感器測量不同點的磁場，用算法計算出無線膠囊內窺鏡的三維位置和鏡頭對準(二維)方向。該技術具有精度高，定位速度快的優點，但無法確定無線膠囊內窺鏡繞主軸旋轉的方向變化信息，這一維信息缺失就不能利用無線膠囊內窺鏡拍攝的圖像進行消化道三維重建；另外，由于永磁體的磁場強度隨距離增加快速衰減，磁場傳感器和磁標記間的有效距離難以滿足人體尺寸要求。

也有人提出基于永磁和感應線圈的定位方法，即利用機械臂控制永磁體的位置和方向，讓永磁體在震動模塊的作用下作往復運動產生變化的磁場，膠囊內的三軸感應線圈輸出感應電動勢。此方法需要在無線膠囊內窺鏡內布置兩個三軸感應線圈才能定位，要增大無線膠囊內窺鏡的體積。

技術實現要素：

為了解決現有技術不足，本發明的目的在于提供一種只采用一個二軸感應線圈，集成方便，占用無線膠囊內窺鏡空間小，可實時連續對無線膠囊內窺鏡定位，方便后續操作的無線膠囊內窺鏡三維位置和三維姿態的定位系統及其定位方法。

為了實現上述發明目的，本發明采用的技術方案是：

該發明的第一發明目的，提供無線膠囊內窺鏡三維位置和三維姿態的定位系統，包括設置于人體外且三軸正交的發射線圈、體外無線接收模塊、體外位姿計算模塊和位于體內的無線膠囊內窺鏡，所述發射線圈由三軸正交的線圈i、線圈ii和線圈iii組成，所述線圈i、線圈ii和線圈iii順序發射各自固定頻率的信號，線圈i、線圈ii、線圈iii發射完一次信號形成一個周期；所述無線膠囊內窺鏡內設置有二軸正交的感應線圈，所述二軸正交的感應線圈對應發射線圈的一個周期內輸出三組不同頻率的電壓信號，且二軸正交的感應線圈輸出的每組不同頻率的電壓信號個數為二個；所述發射線圈與無線膠囊內窺鏡之間通過交變磁場形成磁路，所述無線膠囊內窺鏡與體外無線接收模塊之間通過無線信號連接，所述位姿計算模塊與體外無線接收模塊之間直接連接。

優選地，所述無線膠囊內窺鏡還包括信號放大模塊、ad轉換模塊和無線發送模塊，所述二軸正交的感應線圈與信號放大模塊直接連接，所述信號放大模塊與無線發送模塊直接連接。

該發明第二發明目的，提供無線膠囊內窺鏡三維位置和三維姿態的定位方法，包括以下步驟：

步驟1、在人體外設置三軸正交的發射線圈，在無線膠囊內窺鏡內設置二軸正交的感應線圈并隨無線膠囊內窺鏡進入人體內，發射線圈由三軸正交的線圈i、線圈ii和線圈iii組成；

步驟2、發射線圈所在三個軸建立的坐標系oxyz作為參考坐標系，感應線圈二個軸的交點設置在無線膠囊內窺鏡中心點，作為無線膠囊內窺鏡的位置，其在參考坐標系中表示為(x,y,z)，感應線圈所在的二個軸建立的坐標系o'uxuyuz作為物體坐標系，用單位向量ux和uy指示無線膠囊內窺鏡的姿態；

步驟3、上電后，發射線圈的線圈i、線圈ii和線圈iii在每一個周期內順序發射各自固定頻率的信號；

步驟4、無線膠囊內窺鏡內的放大模塊放大感應線圈的輸出電壓；

步驟5、無線膠囊內窺鏡內ad轉換模塊采樣經過放大的輸出電壓；

步驟6、無線膠囊內窺鏡內的無線發送模塊發送采樣信號；

步驟7、體外無線接收模塊接收采樣信號，并發送給位姿計算模塊；

步驟8、位姿計算模塊的定位過程如下：

求解9個參數(x,y,z,uxx,uxy,uxz,uyx,uyy,uyz)，其中(uxx,uxy,uxz)和(uyx,uyy,uyz)分別表示ux和uy在參考坐標系的x、y、z軸的投影分量；

將每個軸的發射線圈都等效為磁偶極子，根據畢奧薩法爾定律，磁偶極子在無線膠囊內窺鏡位置處產生的磁通量密度沿參考坐標系的x、y、z軸的三個正交分量如公式(1)、(2)、(3)所示：

其中，(m，n，p)是各軸發射線圈的方向向量，(x，y，z)是感應線圈的位置，(a，b，c)是發射線圈的位置，bt是與發射線圈有關的一個常量，l是感應線圈到發射線圈的距離，l如公式(4)所示：

由于物體坐標系發生了偏轉，物體坐標系的坐標軸與參考坐標系的坐標軸不重合，發射線圈在無線膠囊內窺鏡位置產生的磁通量密度在物體坐標系下的值為公式(5)所示：

其中，r為方位矩陣，如公式(6)所示，由于只有兩個感應線圈，所以b'z不激發線圈輸出電壓；

其中

(uxx,uxy,uxz)＝ux(7)

(uyx,uyy,uyz)＝uy(8)

感應線圈輸出的是感應電壓信號，根據法拉第電磁感應定律，感應線圈產生的感應電動勢為公式(9)所示：

其中，n為感應線圈匝數，φ為穿過曲面s的磁通量；

在ux和uy方向上，感應線圈輸出的電壓信號與磁通量密度之間的關系如下：

由于感器線圈很小，故將其體積忽略，認為感應線圈各處磁通量密度相等，所以公式(10)和(11)變為如下公式(12)和(13)，

因為感應線圈的方向與物體坐標系的坐標軸方向相同，所以得到如下公式(14)和(15)，

若發射已知頻率的正弦信號，物體坐標系下的磁通量密度可以描述為如下公式(16)：

至此，可以得出感應線圈的ux、uy軸輸出電壓值與物體坐標系各軸上磁通量密度之間的關系式，如公式(17)、(18)所示：

感應線圈的輸出電壓信號是與發射信號同頻率的余弦信號，取該信號的幅值來建立方程組，設etx＝-ωxnx·sx，ety＝-ωyny·sy，etz＝-ωznz·sz，得到方程組(19)、(20)如下：

εxmax＝-ωnx·b'xmax·sx＝etx·b'xmax(19)

εymax＝-ωny·b'ymax·sy＝ety·b'ymax(20)

提取余弦信號幅值和相位的方法有快速傅里葉變換或函數擬合方法等；

若三個軸的發射線圈依次激勵各自固定頻率的正弦信號，二軸的感應線圈總計感應到6組交變信號，從而可以建立6個方程，由于要求解9個未知參數，所以還需要3個方程；由于ux和uy取單位向量，并且相互垂直，所以再增加如下3個約束方程，3個方程式(21)、(22)、(23)如下所示：

uxx.uyx+uxy.uyy+uxz.uyz＝0(23)

下面分別對發射線圈的三個軸進行分析：

線圈i對應于參考坐標系的x軸，其位置和方向參數為

(a，b，c)＝(0，0，0)

(m，n，p)＝(1，0，0)

將其帶入公式(1)、(2)和(3)中，得到公式(24)、(25)和(26)如下：

線圈ii對應于參考坐標系的y軸，其位置和方向參數為

(a，b，c)＝(0，0，0)

(m，n，p)＝(0，1，0)

將其帶入公式(1)、(2)和(3)中，得到公式(27)、(28)和(29)如下：

線圈iii對應于參考坐標系的z軸，其位置和方向參數為

(a，b，c)＝(0，0，0)

(m，n，p)＝(0，0，1)

將其帶入公式(1)、(2)和(3)中，得到公式(30)、(31)和(32)如下：

根據公式(5)、(19)和(20)，再定義公式(33)如下：

其中，bixmax、biymax和bizmax分別是三軸發射線圈的線圈i、線圈ii和線圈iii發射時，在感應線圈處產生的磁通量密度沿參考坐標系的x、y、z軸三個分量的幅值，即是bix、biy和biz的幅值；εixmax和εiymax分別是三軸發射線圈的線圈i、線圈ii和線圈iii發射時，無線膠囊內窺鏡內二軸感應線圈的ux和uy軸感應電壓的理論幅值，εizmax沒有感應線圈輸出，所以不參與計算；

設ε'ixmax和ε'iymax分別是發射線圈的線圈i、線圈ii和線圈iii發射時，無線膠囊內窺鏡內二軸感應線圈的ux和uy軸感應電壓的實際輸出值，即測量值，定義誤差e的公式(34)如下:

利用優化算法如levenberg-marquardt或gauss-newton算法等，使e最小，可以求解出無線膠囊內窺鏡的位姿參數(x,y,z,uxx,uxy,uxz,uyx,uyy,uyz)；

步驟9、位姿計算模塊將無線膠囊內窺鏡的位姿信息發送給顯示終端，實時反映當前無線膠囊內窺鏡的位姿，便于操作者觀察或后續應用。

本發明的有益效果是：

本發明采用在人體外設置一個三軸正交的發射線圈，在無線膠囊內窺鏡內只布置一個二軸正交的感應線圈，發射線圈放置在人體附近，發射線圈的線圈i、線圈ii和線圈iii順序發射各自固定頻率的信號，三軸發射線圈發射完一次信號稱為一個周期，二軸感應線圈在一個周期內感應輸出三組不同頻率的電壓信號，從而建立方程組進行無線膠囊內窺鏡三維位置和三維姿態的計算，該方法集成方便、二軸感應線圈占用無線膠囊內窺鏡空間小，能實時連續對無線膠囊內窺鏡定位，方便后續操作，安全可靠、成本低廉。

【附圖說明】

圖1是本發明體外三軸發射線圈及其對應的無線膠囊內窺鏡內部二軸線圈的放大結構示意圖；

圖2是本發明的定位流程圖。

【具體實施方式】

為使本發明的上述目的、特征和優點能夠更加明顯易懂，下面結合附圖對本發明的具體實施方式做詳細的說明。

無線膠囊內窺鏡三維位置和三維姿態的定位系統，如圖1所示，包括設置于人體外且三軸正交的發射線圈、體外無線接收模塊、體外位姿計算模塊和位于體內的無線膠囊內窺鏡，所述發射線圈由三軸正交的線圈i、線圈ii和線圈iii組成，所述線圈i、線圈ii和線圈iii順序發射各自固定頻率的信號，線圈i、線圈ii、線圈iii發射完一次信號形成一個周期；所述無線膠囊內窺鏡內設置有二軸正交的感應線圈，所述二軸正交的感應線圈對應發射線圈的一個周期內輸出三組不同頻率的電壓信號，且二軸正交的感應線圈輸出的每組不同頻率的電壓信號個數為二個；所述發射線圈與無線膠囊內窺鏡之間通過交變磁場形成磁路，所述無線膠囊內窺鏡與體外無線接收模塊之間通過無線信號連接，所述位姿計算模塊與體外無線接收模塊之間直接連接。

其中，無線膠囊內窺鏡還包括信號放大模塊、ad轉換模塊和無線發送模塊，所述二軸正交的感應線圈與信號放大模塊直接連接，所述信號放大模塊與ad轉換模塊直接連接，所述ad轉換模塊與無線發送模塊直接連接。

該實施例的無線膠囊內窺鏡三維位置和三維姿態的定位方法，如圖2所示，包括以下步驟：

步驟1、在人體外設置三軸正交的發射線圈，在無線膠囊內窺鏡內設置二軸正交的感應線圈并隨無線膠囊內窺鏡進入人體內，發射線圈由三軸正交的線圈i、線圈ii和線圈iii組成；

步驟2、發射線圈所在三個軸建立的坐標系oxyz作為參考坐標系，感應線圈二個軸的交點設置在無線膠囊內窺鏡中心點，作為無線膠囊內窺鏡的位置，其在參考坐標系中表示為(x,y,z)，感應線圈所在的二個軸建立的坐標系o'uxuyuz作為物體坐標系，用單位向量ux和uy指示無線膠囊內窺鏡的姿態；

步驟3、上電后，發射線圈的線圈i、線圈ii和線圈iii在每一個周期內順序發射各自固定頻率的信號；

步驟4、無線膠囊內窺鏡內的放大模塊放大感應線圈的輸出電壓；

步驟5、無線膠囊內窺鏡內ad轉換模塊采樣經過放大的輸出電壓；

步驟6、無線膠囊內窺鏡內的無線發送模塊發送采樣信號；

步驟7、體外無線接收模塊接收采樣信號，并發送給位姿計算模塊；

步驟8、位姿計算模塊的定位過程如下：

求解9個參數(x,y,z,uxx,uxy,uxz,uyx,uyy,uyz)，其中(uxx,uxy,uxz)和(uyx,uyy,uyz)分別表示ux和uy在參考坐標系的x、y、z軸的投影分量；

將每個軸的發射線圈等效成磁偶極子，根據畢奧薩法爾定律，磁偶極子在無線膠囊內窺鏡位置處產生的磁通量密度沿參考坐標系的x、y、z軸的三個正交分量如公式(1)、(2)、(3)所示：

其中，(m，n，p)是各軸發射線圈的方向向量，(x，y，z)是感應線圈的位置，(a，b，c)是發射線圈的位置，bt是與發射線圈有關的一個常量，l是感應線圈到發射線圈的距離，l如公式(4)所示：

由于物體坐標系發生了偏轉，物體坐標系的坐標軸與參考坐標系的坐標軸不重合，發射線圈在無線膠囊內窺鏡位置產生的磁通量密度在物體坐標系下的值為公式(5)所示：

其中，r為方位矩陣，如公式(6)所示，由于只有兩個感應線圈，所以b'z不激發線圈輸出電壓；

其中

(uxx,uxy,uxz)＝ux(7)

(uyx,uyy,uyz)＝uy(8)

感應線圈輸出的是感應電壓信號，根據法拉第電磁感應定律，感應線圈產生的感應電動勢為公式(9)所示：

其中，n為感應線圈匝數，φ為穿過曲面s的磁通量；

在ux和uy方向上，感應線圈輸出的電壓信號與磁通量密度之間的關系如下：

由于感器線圈很小，故將其體積忽略，認為感應線圈各處磁通量密度相等，所以公式(10)和(11)變為如下公式(12)和(13)，

因為感應線圈的方向與物體坐標系的坐標軸方向相同，所以得到如下公式(14)和(15)，

若發射已知頻率的正弦信號，也可用其它信號，本發明不限于此，物體坐標系下的磁通量密度可以描述為如下公式(16)：

至此，可以得出感應線圈的ux、uy軸輸出電壓值與物體坐標系各軸上磁通量密度之間的關系式，如公式(17)、(18)所示：

感應線圈的輸出電壓信號是與發射信號同頻率的余弦信號，取該信號的幅值來建立方程組，設etx＝-ωxnx·sx，ety＝-ωyny·sy，etz＝-ωznz·sz，得到方程組(19)、(20)如下：

εxmax＝-ωnx·b'xmax·sx＝etx·b'xmax(19)

εymax＝-ωny·b'ymax·sy＝ety·b'ymax(20)

提取余弦信號幅值和相位的方法有快速傅里葉變換或函數擬合方法，也可用其它方法，本發明不限于此，

三個軸的發射線圈依次激勵各自不同頻率的正弦信號，二軸感應線圈總計感應到6組交變信號，從而可以建立6個方程，由于要求解9個未知參數，所以還需要3個方程；由于ux和uy取單位向量，并且相互垂直，所以再增加如下3個約束方程，3個方程式(21)、(22)、(23)如下所示：

uxx.uyx+uxy.uyy+uxz.uyz＝0(23)

下面分別對發射線圈的三個軸進行分析：

線圈i對應于參考坐標系的x軸，其位置和方向參數為

(a，b，c)＝(0，0，0)

(m，n，p)＝(1，0，0)

將其帶入公式(1)、(2)和(3)中，得到公式(24)、(25)和(26)如下：

線圈ii對應于參考坐標系的y軸，其位置和方向參數為

(a，b，c)＝(0，0，0)

(m，n，p)＝(0，1，0)

將其帶入公式(1)、(2)和(3)中，得到公式(27)、(28)和(29)如下：

線圈iii對應于參考坐標系的z軸，其位置和方向參數為

(a，b，c)＝(0，0，0)

(m，n，p)＝(0，0，1)

將其帶入公式(1)、(2)和(3)中，得到公式(30)、(31)和(32)如下：

根據公式(5)、(19)和(20)，再定義公式(33)如下：

其中，bixmax、biymax和bizmax分別是三軸發射線圈的線圈i、線圈ii和線圈iii發射時，在感應線圈處產生的磁通量密度沿參考坐標系的x、y、z軸三個分量的幅值，即是bix、biy和biz的幅值；εixmax和εiymax分別是三軸發射線圈的線圈i、線圈ii和線圈iii發射時，無線膠囊內窺鏡內二軸感應線圈的ux和uy軸感應電壓的理論幅值，εizmax沒有感應線圈輸出，所以不參與計算；

設εi'xmax和εi'ymax分別是發射線圈的線圈i、線圈ii和線圈iii發射時，無線膠囊內窺鏡內二軸感應線圈的ux和uy軸感應電壓的實際輸出值，即測量值，定義誤差e的公式(34)如下:

利用優化算法如levenberg-marquardt或gauss-newton算法，也可用其它方法，本發明不限于此，使e最小，可以求解出無線膠囊內窺鏡的位姿參數(x,y,z,uxx,uxy,uxz,uyx,uyy,uyz)；

步驟9、位姿計算模塊將無線膠囊內窺鏡的位姿信息發送給顯示終端，實時反映當前無線膠囊內窺鏡的位姿，便于操作者觀察或后續應用。

以上所述實施例只是為本發明的較佳實施例，并非以此限制本發明的實施范圍，除了具體實施例中列舉的情況外，凡依本發明原理所作的等效變化，均應涵蓋于本發明的保護范圍內。