[發(fā)明專(zhuān)利]基于正態(tài)分布和雅可比矩陣的同心管機(jī)器人運(yùn)動(dòng)規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110374981.6
• 申請(qǐng)日: 2021-04-08
• 公開(kāi)（公告）號(hào): CN113282077B
• 公開(kāi)（公告）日: 2022-06-17
• 發(fā)明（設(shè)計(jì)）人: 馮子俊;李永強(qiáng);馮宇;馮遠(yuǎn)靜;劉揚(yáng)
• 申請(qǐng)（專(zhuān)利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類(lèi)號(hào): G05D1/02
• 分類(lèi)號(hào): G05D1/02
• 代理公司: 杭州斯可睿專(zhuān)利事務(wù)所有限公司 33241
• 代理人: 王利強(qiáng)

一種基于正態(tài)分布RRT*和雅可比矩陣的同心管機(jī)器人運(yùn)動(dòng)規(guī)劃方法，根據(jù)同心管機(jī)器人參數(shù)及正運(yùn)動(dòng)學(xué)模型計(jì)算并繪制其工作空間；根據(jù)解剖結(jié)構(gòu)，生成手術(shù)工作環(huán)境點(diǎn)云；利用基于正態(tài)分布RRT*算法生成同心管機(jī)器人最優(yōu)工作路徑；使用基于雅可比的逆運(yùn)動(dòng)學(xué)計(jì)算出路徑上每個(gè)點(diǎn)對(duì)應(yīng)的驅(qū)動(dòng)空間輸入量，使用輸入量作為當(dāng)前步的積分初值，更快速的計(jì)算出了每個(gè)路徑點(diǎn)對(duì)應(yīng)的輸入量q。與現(xiàn)有技術(shù)相比，本發(fā)明能夠在較短時(shí)間內(nèi)生成一條符合手術(shù)和同心管形狀要求的最優(yōu)路徑。

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

本發(fā)明涉及手術(shù)機(jī)器人領(lǐng)域，具體為一種基于正態(tài)分布RRT*和雅可比矩陣的同心管機(jī)器人運(yùn)動(dòng)規(guī)劃方法。

背景技術(shù)

近些年來(lái)，隨著醫(yī)療水平的穩(wěn)步發(fā)展，產(chǎn)生了各種能夠在手術(shù)中發(fā)揮關(guān)鍵作用的機(jī)器人。相比于一般的剛性器械，同心管這一類(lèi)連續(xù)型機(jī)器人靈活彎曲，并沿著術(shù)前規(guī)劃的非線(xiàn)性路徑到達(dá)指定病灶點(diǎn)的能力開(kāi)始受到廣泛關(guān)注。但是，此類(lèi)機(jī)器人的關(guān)節(jié)自由度趨于無(wú)限大，并且其在自由空間中的形狀十分復(fù)雜，從而導(dǎo)致其運(yùn)動(dòng)規(guī)劃的復(fù)雜程度顯著上升。

同心管的建模設(shè)計(jì)大都建立在分段常曲率和剛度主宰的假設(shè)上，并且忽略了剪切應(yīng)變以及摩擦等難以量化的信息。在此基礎(chǔ)上使用靜力學(xué)平衡法和能量最小原則建立的同心管正運(yùn)動(dòng)學(xué)模型已能達(dá)到較高的精度，而在路徑規(guī)劃方面，普遍適用于高維空間的采樣路徑規(guī)劃算法受到了人們的廣泛關(guān)注，如PRM和RRT。

現(xiàn)在仍有許多問(wèn)題阻礙著同心管機(jī)器人系統(tǒng)的進(jìn)一步發(fā)展：同心管的原材料鎳鈦合金管加工難度很大，且會(huì)隨著時(shí)間推移發(fā)生老化和變形，無(wú)法滿(mǎn)足運(yùn)動(dòng)學(xué)模型的理想化假設(shè)要求以及手術(shù)的高精度要求。

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

為了克服現(xiàn)有技術(shù)的不足，為了在一定程度上提高同心管系統(tǒng)的整體運(yùn)行能力，本發(fā)明提出了一種基于正態(tài)分布RRT*和雅可比矩陣的同心管機(jī)器人運(yùn)動(dòng)規(guī)劃方法，能夠提高機(jī)器人在復(fù)雜手術(shù)環(huán)境下的工作效率。

本發(fā)明所采用的技術(shù)方案是：

一種基于正態(tài)分布RRT*和雅可比矩陣的同心管機(jī)器人運(yùn)動(dòng)規(guī)劃方法，包括以下步驟：

1)對(duì)于幾何參數(shù)和力學(xué)參數(shù)已知的同心管機(jī)器人，預(yù)計(jì)算其正運(yùn)動(dòng)學(xué)的解，繪制機(jī)器人的工作空間，建立驅(qū)動(dòng)輸入與末端位姿一一對(duì)應(yīng)的數(shù)據(jù)表

2)通過(guò)安裝在同心管機(jī)器人末端的內(nèi)窺鏡系統(tǒng)生成手術(shù)解剖結(jié)構(gòu)的點(diǎn)云，并將相互距離小于同心管截面直徑d的點(diǎn)用一個(gè)最小球體包裹住，設(shè)此方法生成的障礙物球體共有N個(gè)，且第i個(gè)障礙物的半徑為R_i；

3)使用基于正態(tài)分布的RRT*算法進(jìn)行同心管機(jī)器人的路徑規(guī)劃：從數(shù)據(jù)表中選取出位于病灶點(diǎn)附近且不在障礙物內(nèi)的位置x_target，并根據(jù)其對(duì)應(yīng)的輸入，利用正運(yùn)動(dòng)學(xué)模型繪制出機(jī)器人在工作空間中的整體形狀Shape，該形狀對(duì)應(yīng)的點(diǎn)集為S0，因?yàn)橥墓艿慕M(mǎn)足剛度主宰的假設(shè)，故若S0中的任意一點(diǎn)都不在障礙物內(nèi)，則同心管在該手術(shù)環(huán)境中的最優(yōu)工作路徑即為S0；若存在一段路徑被第i個(gè)障礙物截?cái)?，則需要對(duì)該段進(jìn)行新的路徑規(guī)劃；

4)根據(jù)步驟3)實(shí)現(xiàn)了障礙物段的最優(yōu)路徑規(guī)劃，得到路徑S2，進(jìn)而得到同心管機(jī)器人在工作空間中的最佳路徑S＝S0-S1+S2。

5)為了使機(jī)器人能夠沿著路徑S到達(dá)病灶點(diǎn)，使用基于雅可比矩陣J的逆運(yùn)動(dòng)學(xué)逐步計(jì)算出路徑上每一個(gè)點(diǎn)x所對(duì)應(yīng)的機(jī)器人驅(qū)動(dòng)輸入量q；

6)使用步驟5)計(jì)算得到的輸入量作為當(dāng)前步的積分初值，更快速的計(jì)算出了每個(gè)路徑點(diǎn)對(duì)應(yīng)的輸入量q。

進(jìn)一步，所述步驟3)的過(guò)程如下：