[發(fā)明專利]一種膠囊內(nèi)窺鏡運動控制方法、裝置及終端設(shè)備

在審

• 申請?zhí)枺?/td>: 201810984388.1
• 申請日: 2018-08-28
• 公開（公告）號: CN109044250A
• 公開（公告）日: 2018-12-21
• 發(fā)明（設(shè)計）人: 胡進;茹澤偉;王東遠(yuǎn);嚴(yán)寒
• 申請（專利權(quán)）人: 深圳市資福醫(yī)療技術(shù)有限公司
• 主分類號: A61B1/04
• 分類號: A61B1/04;A61B1/273;G05B19/042;G05B11/42
• 代理公司: 深圳盛德大業(yè)知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 44333
• 代理人: 賈振勇

1.一種膠囊內(nèi)窺鏡運動控制方法，其特征在于，所述方法包括：

獲取膠囊內(nèi)窺鏡當(dāng)前時間點的實際位置和實際姿態(tài)，以及當(dāng)前時間點對應(yīng)的目標(biāo)位置和目標(biāo)姿態(tài)；

根據(jù)預(yù)設(shè)的運動控制算法，結(jié)合所述膠囊內(nèi)窺鏡的實際位置和實際姿態(tài)、目標(biāo)位置和目標(biāo)姿態(tài)以及磁控設(shè)備對膠囊內(nèi)窺鏡的磁力模型，計算出所述磁控設(shè)備的運動軌跡，其中所述磁控設(shè)備為控制所述膠囊內(nèi)窺鏡運動的設(shè)備；

所述磁控設(shè)備沿著所述運動軌跡運動，以控制所述膠囊內(nèi)窺鏡運動。

2.如權(quán)利要求1所述的膠囊內(nèi)窺鏡運動控制方法，其特征在于，在獲取膠囊內(nèi)窺鏡當(dāng)前時間點的實際位置和實際姿態(tài)前，還包括：

根據(jù)用戶輸入的交互指令規(guī)劃所述膠囊內(nèi)窺鏡的目標(biāo)運動軌跡；

所述當(dāng)前時間點對應(yīng)的目標(biāo)位置和目標(biāo)姿態(tài)為所述目標(biāo)運動軌跡上當(dāng)前時間點對應(yīng)的位置和姿態(tài)。

3.如權(quán)利要求1所述的膠囊內(nèi)窺鏡運動控制方法，其特征在于，所述方法還包括：

對所述膠囊內(nèi)窺鏡進行觸壁檢測，若檢測到所述膠囊內(nèi)窺鏡觸壁，則重新規(guī)劃所述目標(biāo)運動軌跡。

4.如權(quán)利要求1所述的膠囊內(nèi)窺鏡運動控制方法，其特征在于，所述獲取膠囊內(nèi)窺鏡當(dāng)前時間點的實際位置和實際姿態(tài)包括：

通過位姿檢測模塊獲取所述膠囊內(nèi)窺鏡當(dāng)前時間點的實際位置和實際姿態(tài)，其中所述位姿檢測模塊設(shè)置在所述膠囊內(nèi)窺鏡上。

5.如權(quán)利要求1所述的膠囊內(nèi)窺鏡運動控制方法，其特征在于，所述根據(jù)預(yù)設(shè)的運動控制算法，結(jié)合所述膠囊內(nèi)窺鏡的實際位置和實際姿態(tài)、目標(biāo)位置和目標(biāo)姿態(tài)以及磁控設(shè)備對膠囊內(nèi)窺鏡的磁力模型，計算出所述磁控設(shè)備的運動軌跡，其中所述磁控設(shè)備為控制所述膠囊內(nèi)窺鏡運動的設(shè)備包括：

根據(jù)重力前饋補償算法將所述膠囊內(nèi)窺鏡的實際位置校正至磁力重力平衡；

根據(jù)PID反饋算法，結(jié)合所述膠囊內(nèi)窺鏡的目標(biāo)位置和目標(biāo)姿態(tài)、實際位置和實際姿態(tài)以及磁控設(shè)備對膠囊內(nèi)窺鏡的磁力模型計算出所述磁控設(shè)備的運動軌跡。

6.一種膠囊內(nèi)窺鏡運動控制裝置，其特征在于，所述裝置包括：

獲取模塊，用于獲取膠囊內(nèi)窺鏡當(dāng)前時間點的實際位置和實際姿態(tài)，以及當(dāng)前時間點對應(yīng)的目標(biāo)位置和目標(biāo)姿態(tài)；

算法模塊，用于根據(jù)預(yù)設(shè)的運動控制算法，結(jié)合所述膠囊內(nèi)窺鏡的實際位置和實際姿態(tài)、目標(biāo)位置和目標(biāo)姿態(tài)以及磁控設(shè)備對膠囊內(nèi)窺鏡的磁力模型，計算出所述磁控設(shè)備的運動軌跡，其中所述磁控設(shè)備為控制所述膠囊內(nèi)窺鏡運動的設(shè)備；

運動模塊，用于所述磁控設(shè)備沿著所述運動軌跡運動，以控制所述膠囊內(nèi)窺鏡運動。

7.如權(quán)利要求6所述的膠囊內(nèi)窺鏡運動控制裝置，其特征在于，所述裝置還包括：

目標(biāo)軌跡模塊，用于根據(jù)用戶輸入的交互指令規(guī)劃所述膠囊內(nèi)窺鏡的目標(biāo)運動軌跡。

8.如權(quán)利要求6所述的膠囊內(nèi)窺鏡運動控制裝置，其特征在于，所述裝置還包括：

觸壁檢測模塊，用于對所述膠囊內(nèi)窺鏡進行觸壁檢測，若檢測到所述膠囊內(nèi)窺鏡觸壁，則重新規(guī)劃所述目標(biāo)運動軌跡。

9.如權(quán)利要求6所述的膠囊內(nèi)窺鏡運動控制裝置，其特征在于，所述獲取模塊包括：

位姿檢測模塊，用于獲取所述膠囊內(nèi)窺鏡的實際位置和實際姿態(tài)。

10.如權(quán)利要求6所述的膠囊內(nèi)窺鏡運動控制裝置，其特征在于，所述算法模塊包括：

磁力重力校正單元，用于根據(jù)重力前饋補償算法將所述膠囊內(nèi)窺鏡的實際位置校正至磁力重力平衡；

軌跡計算單元，用于根據(jù)PID反饋算法，結(jié)合所述膠囊內(nèi)窺鏡的目標(biāo)位置和目標(biāo)姿態(tài)、實際位置和實際姿態(tài)以及磁控設(shè)備對膠囊內(nèi)窺鏡的磁力模型計算出所述磁控設(shè)備的運動軌跡。

11.一種膠囊內(nèi)窺鏡運動控制終端設(shè)備，包括存儲器、處理器以及存儲在所述存儲器中并可在所述處理器上運行的計算機程序，其特征在于，所述處理器執(zhí)行所述計算機程序時實現(xiàn)如權(quán)利要求1至5任一項所述方法的步驟。

12.一種計算機可讀存儲介質(zhì)，所述計算機可讀存儲介質(zhì)存儲有計算機程序，其特征在于，所述計算機程序被處理器執(zhí)行時實現(xiàn)如權(quán)利要求1至5任一項所述方法的步驟。