本發(fā)明涉及醫(yī)療技術(shù)領(lǐng)域，特別涉及一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定方法。本發(fā)明還涉及一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定系統(tǒng)以及一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定裝置。

背景技術(shù)：

隨著微光機(jī)電技術(shù)、機(jī)器人技術(shù)、無(wú)線通信技術(shù)的發(fā)展，醫(yī)療儀器開始向微型化、智能化、無(wú)創(chuàng)診查方向發(fā)展，電子膠囊逐漸成為研究熱點(diǎn)。膠囊內(nèi)窺鏡極大地拓展了醫(yī)生的消化道檢查視野，填補(bǔ)了胃鏡、腸鏡檢查的盲區(qū)，解決了多年來(lái)小腸疾病和胃腸道隱血診斷方面的難題。與傳統(tǒng)醫(yī)用內(nèi)窺鏡相比，膠囊內(nèi)窺鏡具有操作簡(jiǎn)單、檢查方便、無(wú)創(chuàng)傷、無(wú)痛苦、無(wú)交叉感染、不影響患者的正常工作等優(yōu)點(diǎn)，尤其對(duì)小腸可疑性病變具有很高的診斷價(jià)值，被醫(yī)學(xué)界譽(yù)為21世紀(jì)內(nèi)窺鏡發(fā)展的革命與方向。

而目前廣泛使用外部磁場(chǎng)驅(qū)動(dòng)方式的主動(dòng)可控式膠囊內(nèi)窺鏡系統(tǒng)，該系統(tǒng)包括內(nèi)置在膠囊內(nèi)窺鏡內(nèi)的永磁體或在膠囊內(nèi)窺鏡外表面上包覆永磁體層，和外部磁場(chǎng)裝置，該外部磁場(chǎng)裝置可以采用能夠通過(guò)人工控制的磁力臂，或者也可以采用三軸亥姆霍茲線圈，通過(guò)該三軸亥姆霍茲線圈的萬(wàn)向均勻旋轉(zhuǎn)磁場(chǎng)驅(qū)動(dòng)膠囊內(nèi)窺鏡進(jìn)行滾動(dòng)、俯仰、偏航和前進(jìn)等運(yùn)動(dòng)。

在主動(dòng)式膠囊內(nèi)窺鏡(以后簡(jiǎn)稱膠囊)的使用中，膠囊空間姿態(tài)的全解析對(duì)于檢測(cè)過(guò)程來(lái)說(shuō)十分有必要，膠囊空間姿態(tài)的全解析能夠帶來(lái)以下技術(shù)優(yōu)勢(shì)：輔助操作者或者是計(jì)算機(jī)進(jìn)行膠囊指向位置判定、幫助操作者或者是計(jì)算機(jī)進(jìn)行后續(xù)檢查路線的規(guī)劃、為膠囊空間定位提供必要的姿態(tài)數(shù)據(jù)。

目前，膠囊的空間姿態(tài)全解析也存在著困難，表現(xiàn)為：膠囊工作環(huán)境于外部完全視覺(jué)隔離，使得大部分依賴視頻識(shí)別姿態(tài)測(cè)量手段無(wú)效；人體組織對(duì)于電磁波的衰減效應(yīng)使得大部分依賴于電磁波的姿態(tài)測(cè)量手段失效；由于成本問(wèn)題和用戶體驗(yàn)問(wèn)題，使得放射性成像和核磁共振造影等手段不適用。在使用環(huán)境中可行的方法就是，膠囊內(nèi)部集成傳感器，對(duì)自身的各個(gè)姿態(tài)角進(jìn)行測(cè)量，然后將數(shù)據(jù)實(shí)時(shí)的傳遞出來(lái)，在上位計(jì)算機(jī)中解析出膠囊的全姿態(tài)信息。

在現(xiàn)有技術(shù)中，比較成熟的測(cè)量剛體全姿態(tài)信息的方法一般是使用3軸加速度計(jì)+3軸角速度儀+3軸地磁傳感器。其中，3軸加速度計(jì)和3軸角速度儀用來(lái)完成主要的測(cè)量工作，通過(guò)兩者的數(shù)據(jù)融合，即可得到剛體在各個(gè)軸方向上受到的加速度大小以及剛體繞著各個(gè)軸旋轉(zhuǎn)的角度信息。兩軸數(shù)據(jù)融合有一個(gè)巨大的缺陷，就是3軸角速度儀的數(shù)據(jù)在換算成角度的時(shí)候，存在一個(gè)積分累計(jì)誤差。隨著時(shí)間的推移，測(cè)量的姿態(tài)精度會(huì)變得越來(lái)越不可控。所以此時(shí)使用3軸的地磁傳感器所測(cè)得的地磁向量對(duì)剛體角度進(jìn)行校準(zhǔn)，即可消除累計(jì)誤差。此種9軸數(shù)據(jù)融合的姿態(tài)測(cè)量方法廣泛應(yīng)用于飛控、航空、導(dǎo)航等領(lǐng)域。

然而，膠囊使用內(nèi)部傳感器進(jìn)行全姿態(tài)信息測(cè)量時(shí)，由于主動(dòng)式膠囊內(nèi)窺鏡的使用環(huán)境中，其驅(qū)動(dòng)力來(lái)自于外部磁場(chǎng)對(duì)內(nèi)部磁體的作用力，所以存在很強(qiáng)的磁場(chǎng)干擾，3軸的地磁傳感器無(wú)法使用，并且使用加速度計(jì)和角速度儀進(jìn)行數(shù)據(jù)融合產(chǎn)生的積分累計(jì)誤差是不可接受的。

因此，如何順利完全對(duì)膠囊內(nèi)窺鏡的全姿態(tài)測(cè)定，并提高測(cè)量精度，是本領(lǐng)域技術(shù)人員亟待解決的技術(shù)問(wèn)題。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定方法，能夠順利完成對(duì)膠囊內(nèi)窺鏡的全姿態(tài)測(cè)定，并提高測(cè)量精度。本發(fā)明的另一目的是提供一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定系統(tǒng)以及一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定裝置。

為解決上述技術(shù)問(wèn)題，本發(fā)明提供一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定方法，包括：

檢測(cè)膠囊內(nèi)窺鏡當(dāng)前的合加速度向量；

檢測(cè)所述膠囊內(nèi)窺鏡當(dāng)前位置處由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量；

將所述合加速度向量和磁場(chǎng)向量代入預(yù)設(shè)公式計(jì)算所述膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角。

優(yōu)選地，檢測(cè)所述膠囊內(nèi)窺鏡當(dāng)前的合加速度向量，具體包括：

通過(guò)設(shè)置于所述膠囊內(nèi)窺鏡內(nèi)的三軸加速度傳感器檢測(cè)其預(yù)設(shè)三軸方向上的加速度，再將各軸上的加速度相加計(jì)算所述合加速度向量。

優(yōu)選地，檢測(cè)所述膠囊內(nèi)窺鏡當(dāng)前位置處由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量，具體包括：

通過(guò)設(shè)置于所述膠囊內(nèi)窺鏡內(nèi)的磁場(chǎng)傳感器檢測(cè)所述外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量。

優(yōu)選地，將所述合加速度向量和磁場(chǎng)向量代入預(yù)設(shè)公式計(jì)算所述膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角，具體包括：

通過(guò)公式

計(jì)算所述膠囊內(nèi)窺鏡的俯仰角pitch；

通過(guò)公式

計(jì)算所述膠囊內(nèi)窺鏡的橫滾角roll；

通過(guò)公式

yaw＝β+θ

計(jì)算所述膠囊內(nèi)窺鏡的偏航角yaw；且

其中，

β為外部預(yù)設(shè)定向磁場(chǎng)在水平方向上的偏轉(zhuǎn)角度；

θ為與在水平方向上的夾角；

為所述膠囊內(nèi)窺鏡的x軸基向量，設(shè)

為所述膠囊內(nèi)窺鏡的y軸基向量，設(shè)

為所述膠囊內(nèi)窺鏡的z軸基向量，設(shè)

為所述膠囊內(nèi)窺鏡的合加速度向量，設(shè)

為所述膠囊內(nèi)窺鏡當(dāng)前所處位置的由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量，設(shè)

為在所述膠囊內(nèi)窺鏡的x-y平面上的投影向量，設(shè)

且

當(dāng)且a∈r,a>0時(shí)，roll＝|roll|，

當(dāng)且a∈r,a≤0時(shí)，roll＝-|roll|；

當(dāng)且a∈r,a>0時(shí)，θ＝|θ|，

當(dāng)且a∈r,a≤0時(shí)，θ＝-|θ|。

本發(fā)明還提供一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定系統(tǒng)，包括

第一檢測(cè)模塊，用于檢測(cè)膠囊內(nèi)窺鏡當(dāng)前的合加速度向量；

第二檢測(cè)模塊，用于檢測(cè)所述膠囊內(nèi)窺鏡當(dāng)前位置處由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量；

計(jì)算模塊，用于將所述合加速度向量和磁場(chǎng)向量代入預(yù)設(shè)公式計(jì)算所述膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角。

優(yōu)選地，所述第一檢測(cè)模塊具體包括設(shè)置于所述膠囊內(nèi)窺鏡內(nèi)、用于檢測(cè)其預(yù)設(shè)三軸方向上加速度的三軸加速度傳感器。

優(yōu)選地，所述第二檢測(cè)模塊具體包括設(shè)置于所述膠囊內(nèi)窺鏡內(nèi)、用于檢測(cè)其當(dāng)前位置處由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量的磁場(chǎng)傳感器。

本發(fā)明還提供一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定裝置，包括用于進(jìn)入病患體內(nèi)進(jìn)行圖像采集的膠囊內(nèi)窺鏡、用于通過(guò)磁力吸引控制所述膠囊內(nèi)窺鏡運(yùn)動(dòng)狀態(tài)的控制磁場(chǎng)發(fā)生器，以及用于在病患體表周圍形成定向磁場(chǎng)的測(cè)定磁場(chǎng)發(fā)生器。

本發(fā)明所提供的膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定方法，主要包括三個(gè)步驟，分別為：檢測(cè)膠囊內(nèi)窺鏡當(dāng)前的合加速度向量；檢測(cè)所述膠囊內(nèi)窺鏡當(dāng)前位置處由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量；將所述合加速度向量和磁場(chǎng)向量代入預(yù)設(shè)公式計(jì)算所述膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角。其中，在第一步中，首先檢測(cè)膠囊內(nèi)窺鏡在進(jìn)入人體后在各個(gè)方向上受到的加速度和合加速度(包括數(shù)值和方向)，因此為合加速度向量。在第二步中，膠囊內(nèi)窺鏡的外部空間內(nèi)預(yù)先設(shè)置有定向磁場(chǎng)(該磁感線方向在水平方向上平行或部分區(qū)域具有較高平行度，比如95％以上等，且與預(yù)設(shè)的水平軸具有一定偏轉(zhuǎn)角度)，便于定位膠囊內(nèi)窺鏡的姿態(tài)，在本步驟中檢測(cè)膠囊內(nèi)窺鏡在其當(dāng)前位置處的磁場(chǎng)向量。在第三步中，結(jié)合膠囊內(nèi)窺鏡的當(dāng)前合加速度向量與檢測(cè)到的當(dāng)前磁場(chǎng)向量，即可根據(jù)該兩種數(shù)據(jù)確定膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角(也稱“歐拉角”，eulerangle)。其中，根據(jù)運(yùn)動(dòng)學(xué)領(lǐng)域的公式，膠囊內(nèi)窺鏡的合加速度向量與其3個(gè)方向的移動(dòng)自由度相關(guān)，用于確定其位置，而膠囊內(nèi)窺鏡在其位置處檢測(cè)的磁場(chǎng)向量與其3個(gè)方向的轉(zhuǎn)動(dòng)自由度相關(guān)，用于確定其姿態(tài)。因此，兩者結(jié)合計(jì)算，即可順利完成對(duì)膠囊內(nèi)窺鏡的全姿態(tài)測(cè)定。同時(shí)，相比于現(xiàn)有技術(shù)，本發(fā)明中的磁場(chǎng)向量由外部預(yù)先設(shè)置的定向磁場(chǎng)產(chǎn)生，磁場(chǎng)強(qiáng)度較高，且方向固定，可以避免通過(guò)地磁感應(yīng)器檢測(cè)地磁場(chǎng)的磁場(chǎng)向量時(shí)受到的強(qiáng)磁干擾而引起的測(cè)量不精問(wèn)題；并且避開了現(xiàn)有技術(shù)中使用角速度儀的檢測(cè)值來(lái)?yè)Q算角度引起的積分累計(jì)誤差問(wèn)題，提高了姿態(tài)測(cè)量精度。

附圖說(shuō)明

為了更清楚地說(shuō)明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖僅僅是本發(fā)明的實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)提供的附圖獲得其他的附圖。

圖1為本發(fā)明所提供的一種具體實(shí)施方式的流程圖；

圖2為膠囊內(nèi)窺鏡的三軸坐標(biāo)系與檢測(cè)到的合加速度向量、磁場(chǎng)向量的示意圖；

圖3為本發(fā)明所提供的一種具體實(shí)施方式的模塊圖；

圖4為本發(fā)明所提供的一種具體實(shí)施方式的結(jié)構(gòu)圖。

其中，圖4中：

膠囊內(nèi)窺鏡—1，控制磁場(chǎng)發(fā)生器—2，測(cè)定磁場(chǎng)發(fā)生器—3。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例。基于本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

請(qǐng)參考圖1，圖1為本發(fā)明所提供的一種具體實(shí)施方式的流程圖。

在本發(fā)明所提供的一種具體實(shí)施方式中，膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定方法主要包括三個(gè)步驟，分別為：檢測(cè)膠囊內(nèi)窺鏡當(dāng)前的合加速度向量；檢測(cè)所述膠囊內(nèi)窺鏡當(dāng)前位置處由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量；將所述合加速度向量和磁場(chǎng)向量代入預(yù)設(shè)公式計(jì)算所述膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角。

其中，在第一步中，首先檢測(cè)膠囊內(nèi)窺鏡在進(jìn)入人體后在各個(gè)方向上受到受到的加速度和合加速度(包括數(shù)值和方向)，因此為合加速度向量。

具體的，在本步驟中，可以通過(guò)三軸加速度傳感器來(lái)檢測(cè)膠囊內(nèi)窺鏡在其三軸方向上的加速度，此處所指三軸方向，即以膠囊內(nèi)窺鏡為基準(zhǔn)制定的三維坐標(biāo)系，其中的x、y、z軸可在膠囊內(nèi)窺鏡上任意指定，不影響其姿態(tài)的準(zhǔn)確測(cè)定。但為方便論述，本實(shí)施例以下內(nèi)容均以膠囊內(nèi)窺鏡的軸向方向?yàn)閦軸，且為右手坐標(biāo)系為標(biāo)準(zhǔn)進(jìn)行說(shuō)明。一般的，三軸加速度傳感器可設(shè)置在膠囊內(nèi)窺鏡內(nèi)。如此，三軸加速度傳感器檢測(cè)出膠囊內(nèi)窺鏡的每軸方向上的加速度之后，之后即可將三軸上的各個(gè)加速度相加，計(jì)算出合加速度向量。之后再將該合加速度向量與膠囊內(nèi)窺鏡的三軸基向量進(jìn)行融合計(jì)算，即可首先測(cè)定出膠囊內(nèi)窺鏡當(dāng)前的俯仰角和橫滾角。

但膠囊內(nèi)窺鏡的空間姿態(tài)僅由三軸加速度傳感器還不能完全測(cè)定，還需測(cè)定其偏航角。如此，在第二步中，膠囊內(nèi)窺鏡的外部空間中預(yù)先設(shè)置有定向磁場(chǎng)，此處優(yōu)選地，該定向磁場(chǎng)的磁感線方向可在水平方向上平行或者部分區(qū)域具有較高的平行度，比如95％以上等，且與預(yù)設(shè)的水平軸具有一定偏轉(zhuǎn)角度β，比如勻強(qiáng)磁場(chǎng)等。該外部預(yù)設(shè)定向磁場(chǎng)主要用于與三軸加速度傳感器一同測(cè)定膠囊內(nèi)窺鏡的偏航角，在本步驟中，主要檢測(cè)膠囊內(nèi)窺鏡在其當(dāng)前位置處的磁場(chǎng)向量，及其磁場(chǎng)強(qiáng)度和方向。如此，雖然外部空間中預(yù)設(shè)的定向磁場(chǎng)中，磁場(chǎng)方向處處相同甚至磁場(chǎng)也相等，但是對(duì)于膠囊內(nèi)窺鏡的自身坐標(biāo)系而言，當(dāng)其姿態(tài)產(chǎn)生變化時(shí)，在相同位置處檢測(cè)到的磁場(chǎng)向量也會(huì)產(chǎn)生變化。因此，根據(jù)膠囊內(nèi)窺鏡在定向磁場(chǎng)范圍內(nèi)所檢測(cè)到的磁場(chǎng)向量，可以輔助確定膠囊內(nèi)窺鏡的偏航角。

具體的，在本步驟中，可以通過(guò)磁場(chǎng)傳感器來(lái)檢測(cè)當(dāng)前位置的外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量。一般的，該磁場(chǎng)傳感器可設(shè)置在膠囊內(nèi)窺鏡內(nèi)部。

在第三步中，即可結(jié)合膠囊內(nèi)窺鏡的當(dāng)前合加速度向量與檢測(cè)到的當(dāng)前磁場(chǎng)向量精確地計(jì)算出膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角(也稱“歐拉角”，eulerangle)。其中，根據(jù)運(yùn)動(dòng)學(xué)領(lǐng)域的公式，膠囊內(nèi)窺鏡的合加速度向量與其3個(gè)方向的移動(dòng)自由度相關(guān)，用于確定其位置，而膠囊內(nèi)窺鏡在其位置處檢測(cè)的磁場(chǎng)向量與其3個(gè)方向的轉(zhuǎn)動(dòng)自由度相關(guān)，用于確定其姿態(tài)。因此，兩者結(jié)合計(jì)算，即可順利完成對(duì)膠囊內(nèi)窺鏡的全姿態(tài)測(cè)定。同時(shí)，相比于現(xiàn)有技術(shù)，本實(shí)施例中的磁場(chǎng)向量由外部預(yù)先設(shè)置的定向磁場(chǎng)產(chǎn)生，磁場(chǎng)強(qiáng)度較高，且方向固定，可以避免通過(guò)地磁感應(yīng)器檢測(cè)地磁場(chǎng)的磁場(chǎng)向量時(shí)受到的強(qiáng)磁干擾而引起的測(cè)量不精問(wèn)題；并且避開了現(xiàn)有技術(shù)中使用角速度儀的檢測(cè)值來(lái)?yè)Q算角度引起的積分累計(jì)誤差問(wèn)題，提高了姿態(tài)測(cè)量精度。

如圖2所示，圖2為膠囊內(nèi)窺鏡的三軸坐標(biāo)系與檢測(cè)到的合加速度向量、磁場(chǎng)向量的示意圖。

具體的，關(guān)于膠囊內(nèi)窺鏡的全姿態(tài)解析計(jì)算過(guò)程，首先可對(duì)膠囊內(nèi)窺鏡進(jìn)行三軸坐標(biāo)系構(gòu)建。本實(shí)施例中均以膠囊內(nèi)窺鏡的軸向方向?yàn)閦軸方向，橫截面方向?yàn)閤-y平面方向?yàn)槔M(jìn)行說(shuō)明。顯然，其余的三軸坐標(biāo)系構(gòu)建方式也是可行的。如此，膠囊內(nèi)窺鏡的x軸基向量即為[100]，而膠囊內(nèi)窺鏡的y軸基向量即為[010]，其z軸基向量即為[001]。同時(shí)可設(shè)檢測(cè)出的合加速度向量為[gxgygz]，檢測(cè)出的磁場(chǎng)向量為[mxmymz]。

因?yàn)橥獠款A(yù)設(shè)定向磁場(chǎng)是根據(jù)定位需要預(yù)先設(shè)定的，因此其磁場(chǎng)方向相對(duì)于預(yù)設(shè)的標(biāo)準(zhǔn)軸的水平方向偏轉(zhuǎn)角度是已知的，設(shè)其角度為β。

同時(shí)，膠囊內(nèi)窺鏡的姿態(tài)由姿態(tài)角確定，而姿態(tài)角主要包括俯仰角、橫滾角和偏航角，因此，可令俯仰角為pitch，橫滾角為roll，偏航角為yaw。

其中，俯仰角pitch可以通過(guò)合加速度向量和膠囊自身坐標(biāo)系z(mì)軸之間的夾角來(lái)求得，所以俯仰角

橫滾角同樣可以通過(guò)合加速度向量求得，設(shè)合加速度向量在膠囊自身坐標(biāo)系xy平面上的投影向量為則為[gxgy0]，且有橫滾角

其中，當(dāng)且a∈r,a>0時(shí)，roll＝|roll|，

當(dāng)且a∈r,a≤0時(shí)，roll＝-|roll|。

偏航角需要通過(guò)合加速度向量和磁場(chǎng)向量進(jìn)行數(shù)據(jù)融合處理，同時(shí)還需要引入外部定向磁場(chǎng)在水平方向上的偏轉(zhuǎn)角度β。

首先設(shè)θ為磁場(chǎng)向量與膠囊內(nèi)窺鏡自身坐標(biāo)系z(mì)軸方向的水平方向夾角，其次，令其中，為與組成平面的法向量，為與組成平面的法向量，如此：

其中，當(dāng)且a∈r,a>0時(shí)，θ＝|θ|，

當(dāng)且a∈r,a≤0時(shí)，θ＝-|θ|；

上述a均為系數(shù)。

θ計(jì)算出之后，將其與β相加即為偏航角yaw。

至此，膠囊內(nèi)窺鏡的俯仰角為pitch，橫滾角為roll，偏航角為yaw均已計(jì)算出來(lái)，可以順利獲知膠囊內(nèi)窺鏡的姿態(tài)角，實(shí)現(xiàn)對(duì)膠囊內(nèi)窺鏡的全姿態(tài)解析。

如圖3所示，圖3為本發(fā)明所提供的一種具體實(shí)施方式的模塊圖。

本實(shí)施例還提供一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定系統(tǒng)，主要包括第一檢測(cè)模塊、第二檢測(cè)模塊和計(jì)算模塊。其中，第一檢測(cè)模塊主要用于檢測(cè)膠囊內(nèi)窺鏡當(dāng)前的合加速度向量，而第二檢測(cè)模塊主要用于檢測(cè)所述膠囊內(nèi)窺鏡當(dāng)前位置處由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量，計(jì)算模塊與第一檢測(cè)模塊和第二檢測(cè)模塊信號(hào)連接，主要用于根據(jù)兩者的檢測(cè)值與預(yù)設(shè)公式計(jì)算膠囊內(nèi)窺鏡的當(dāng)前姿態(tài)角。

在關(guān)于第一檢測(cè)模塊的一種優(yōu)選實(shí)施方式中，該第一檢測(cè)模塊具體可為設(shè)置在膠囊內(nèi)窺鏡內(nèi)的三軸加速度傳感器，主要用于檢測(cè)膠囊內(nèi)窺鏡上預(yù)設(shè)三軸方向上的加速度。

而在關(guān)于第二檢測(cè)模塊的一種優(yōu)選實(shí)施方式中，該第二檢測(cè)模塊具體可為設(shè)置在膠囊內(nèi)窺鏡內(nèi)的磁場(chǎng)傳感器，主要用于檢測(cè)膠囊內(nèi)窺鏡當(dāng)前所處位置由外部預(yù)設(shè)定向磁場(chǎng)產(chǎn)生的磁場(chǎng)向量。

如圖4所示，圖4為本發(fā)明所提供的一種具體實(shí)施方式的結(jié)構(gòu)圖。

本實(shí)施例還提供一種膠囊內(nèi)窺鏡空間姿態(tài)測(cè)定裝置，主要包括膠囊內(nèi)窺鏡1、控制磁場(chǎng)發(fā)生器2和測(cè)定磁場(chǎng)發(fā)生器3。其中，膠囊內(nèi)窺鏡1主要用于進(jìn)入到病患體內(nèi)進(jìn)行圖像采集，在膠囊內(nèi)窺鏡1內(nèi)一般設(shè)置有磁鐵、通訊模塊、控制模塊等，其中磁鐵用于與外部的控制磁場(chǎng)發(fā)生器2產(chǎn)生磁力吸引作用，使得控制磁場(chǎng)發(fā)生器2通過(guò)磁場(chǎng)變化牽引膠囊內(nèi)窺鏡1內(nèi)的磁鐵，進(jìn)而驅(qū)動(dòng)膠囊內(nèi)窺鏡1進(jìn)行運(yùn)動(dòng)。而測(cè)定磁場(chǎng)發(fā)生器3也用于產(chǎn)生定向磁場(chǎng)，不同的是，測(cè)定磁場(chǎng)發(fā)生器3產(chǎn)生的是籠罩在病患體表周圍的定向磁場(chǎng)，并且也不是為了牽引膠囊內(nèi)窺鏡1內(nèi)的磁鐵，而是作為地磁場(chǎng)的替代品，輔助膠囊內(nèi)窺鏡1內(nèi)的加速度傳感器進(jìn)行對(duì)膠囊內(nèi)窺鏡1的姿態(tài)測(cè)定，提高測(cè)定精度。

另外，控制磁場(chǎng)發(fā)生器2與測(cè)定磁場(chǎng)發(fā)生器3還可以一體成型設(shè)計(jì)，如此，其產(chǎn)生的磁場(chǎng)將由其控制器決定，比如，在需要正常牽引膠囊內(nèi)窺鏡1進(jìn)行運(yùn)動(dòng)時(shí)，則使控制磁場(chǎng)發(fā)生器2運(yùn)行，并產(chǎn)生對(duì)應(yīng)磁場(chǎng)；而在需要測(cè)定膠囊內(nèi)窺鏡1的姿態(tài)時(shí)，則使測(cè)定磁場(chǎng)發(fā)生器3運(yùn)行，并產(chǎn)生定向磁場(chǎng)。

對(duì)所公開的實(shí)施例的上述說(shuō)明，使本領(lǐng)域?qū)I(yè)技術(shù)人員能夠?qū)崿F(xiàn)或使用本發(fā)明。對(duì)這些實(shí)施例的多種修改對(duì)本領(lǐng)域的專業(yè)技術(shù)人員來(lái)說(shuō)將是顯而易見(jiàn)的，本文中所定義的一般原理可以在不脫離本發(fā)明的精神或范圍的情況下，在其它實(shí)施例中實(shí)現(xiàn)。因此，本發(fā)明將不會(huì)被限制于本文所示的這些實(shí)施例，而是要符合與本文所公開的原理和新穎特點(diǎn)相一致的最寬的范圍。