[發(fā)明專利]導(dǎo)管運(yùn)動控制方法、裝置、系統(tǒng)和存儲介質(zhì)

在審

• 申請?zhí)枺?/td>: 202310036831.3
• 申請日: 2023-01-10
• 公開（公告）號: CN116370070A
• 公開（公告）日: 2023-07-04
• 發(fā)明（設(shè)計）人: 請求不公布姓名
• 申請（專利權(quán)）人: 上海微創(chuàng)微航機(jī)器人有限公司
• 主分類號: A61B34/00
• 分類號: A61B34/00;A61M25/01;G06T7/73;G06T7/246;G06T19/00;A61M25/09
• 代理公司: 華進(jìn)聯(lián)合專利商標(biāo)代理有限公司 44224
• 代理人: 楊歡

本申請涉及一種導(dǎo)管運(yùn)動控制方法、裝置、系統(tǒng)、存儲介質(zhì)和計算機(jī)程序產(chǎn)品。所述方法包括：周期性獲取導(dǎo)管控制器中控制桿在控制坐標(biāo)系的投影位置；基于控制桿的當(dāng)前周期的投影位置，確定導(dǎo)管的運(yùn)動狀態(tài)；確定導(dǎo)管的活動末端在導(dǎo)管坐標(biāo)系中的導(dǎo)管末端投影位置，根據(jù)導(dǎo)管的運(yùn)動狀態(tài)以及導(dǎo)管末端投影位置，得到導(dǎo)管位置；根據(jù)導(dǎo)管位置，通過逆運(yùn)動學(xué)算法，得到電機(jī)角位移；基于電機(jī)角位移控制電機(jī)轉(zhuǎn)動，以使得與導(dǎo)絲帶動導(dǎo)管的活動末端運(yùn)動。采用本方法能夠提高導(dǎo)管運(yùn)動控制的準(zhǔn)確性。

技術(shù)領(lǐng)域

本申請涉及機(jī)器人控制技術(shù)領(lǐng)域，特別是涉及一種導(dǎo)管運(yùn)動控制方法、裝置、系統(tǒng)、存儲介質(zhì)和計算機(jī)程序產(chǎn)品。

背景技術(shù)

支氣管機(jī)器人中的導(dǎo)管進(jìn)入目標(biāo)對象中，需要通過控制導(dǎo)管的活動末端來改變導(dǎo)管在目標(biāo)對象中的位置。現(xiàn)有的導(dǎo)管運(yùn)動控制方法，往往采用軌跡球進(jìn)行導(dǎo)管兩向彎曲運(yùn)動控制，或者采用手柄控制兩級鞘管實現(xiàn)導(dǎo)管的運(yùn)動控制。然而這種方法只考慮簡單的位置映射，沒有精細(xì)的參數(shù)調(diào)節(jié)，存在導(dǎo)管運(yùn)動控制不準(zhǔn)確的問題。

發(fā)明內(nèi)容

基于此，有必要針對傳統(tǒng)導(dǎo)管運(yùn)動控制不準(zhǔn)確的問題，提供一種能夠提高導(dǎo)管運(yùn)動控制準(zhǔn)確性的導(dǎo)管運(yùn)動控制方法、裝置、系統(tǒng)、計算機(jī)可讀存儲介質(zhì)和計算機(jī)程序產(chǎn)品。

第一方面，本申請?zhí)峁┝艘环N導(dǎo)管運(yùn)動控制方法，所述方法包括：

周期性獲取導(dǎo)管控制器中控制桿在控制坐標(biāo)系的投影位置；

基于控制桿的當(dāng)前周期的投影位置，確定導(dǎo)管的運(yùn)動狀態(tài)；

確定導(dǎo)管的活動末端在導(dǎo)管坐標(biāo)系中的導(dǎo)管末端投影位置，根據(jù)導(dǎo)管的運(yùn)動狀態(tài)以及導(dǎo)管末端投影位置，得到導(dǎo)管位置；

根據(jù)導(dǎo)管位置，通過逆運(yùn)動學(xué)算法，得到電機(jī)角位移；

基于電機(jī)角位移控制電機(jī)轉(zhuǎn)動，以使得導(dǎo)絲帶動導(dǎo)管的活動末端運(yùn)動。

在其中一個實施例中，基于控制桿的當(dāng)前周期的投影位置，確定導(dǎo)管的運(yùn)動狀態(tài)，包括：

若當(dāng)前周期的投影位置為控制桿坐標(biāo)系的坐標(biāo)圓心，則確定導(dǎo)管的運(yùn)動狀態(tài)為不運(yùn)動；

若當(dāng)前周期的投影位置在控制桿坐標(biāo)系的橫軸存在分量，則確定導(dǎo)管的運(yùn)動狀態(tài)為朝導(dǎo)管坐標(biāo)系的橫軸彎曲；

若當(dāng)前周期的投影位置在控制桿坐標(biāo)系的縱軸中存在分量，則確定導(dǎo)管的運(yùn)動狀態(tài)為朝導(dǎo)管坐標(biāo)系的縱軸彎曲。

在其中一個實施例中，根據(jù)導(dǎo)管的運(yùn)動狀態(tài)以及導(dǎo)管末端投影位置，得到導(dǎo)管位置，包括：

獲取控制桿的活動末端在控制桿坐標(biāo)系中的上一周期投影位置；

根據(jù)上一周期投影位置和當(dāng)前周期的投影位置，確定控制桿投影位移；

根據(jù)控制桿投影位移和周期時長，確定控制桿速度；

根據(jù)控制桿速度和導(dǎo)管控制周期，確定導(dǎo)管末端增量位移；

根據(jù)導(dǎo)管的運(yùn)動狀態(tài)、導(dǎo)管末端投影位置和導(dǎo)管末端增量位移，得到導(dǎo)管位置。

在其中一個實施例中，導(dǎo)管末端投影位置為極坐標(biāo)；根據(jù)導(dǎo)管的運(yùn)動狀態(tài)、導(dǎo)管末端投影位置和導(dǎo)管末端增量位移，得到導(dǎo)管位置，包括：

獲取導(dǎo)管末端投影位置在導(dǎo)管坐標(biāo)系中的導(dǎo)管彎曲方向；

獲取導(dǎo)管末端增量位移在導(dǎo)管彎曲方向上的第一投影增量以及在垂直于導(dǎo)管彎曲方向上的第二投影增量；

基于導(dǎo)管的運(yùn)動狀態(tài)、第一投影增量、第二投影增量以及導(dǎo)管末端投影位置，確定導(dǎo)管位置。

在其中一個實施例中，導(dǎo)管運(yùn)動控制方法還包括：