[發(fā)明專利]一種繩驅(qū)蛇形機(jī)械臂路徑規(guī)劃首尾運(yùn)動(dòng)跟隨方法、裝置、計(jì)算機(jī)及存儲(chǔ)介質(zhì)

在審

• 申請(qǐng)?zhí)枺?/td>: 202210092850.3
• 申請(qǐng)日: 2022-01-26
• 公開（公告）號(hào): CN114603539A
• 公開（公告）日: 2022-06-10
• 發(fā)明（設(shè)計(jì)）人: 谷海宇;黃倬然;向陽(yáng);劉鵬;魏承
• 申請(qǐng)（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)
• 主分類號(hào): B25J9/06
• 分類號(hào): B25J9/06;B25J9/16
• 代理公司: 哈爾濱市陽(yáng)光惠遠(yuǎn)知識(shí)產(chǎn)權(quán)代理有限公司 23211
• 代理人: 張宏威

一種繩驅(qū)蛇形機(jī)械臂路徑規(guī)劃運(yùn)動(dòng)跟隨方法、裝置、計(jì)算機(jī)及存儲(chǔ)介質(zhì)，屬于路徑規(guī)劃領(lǐng)域。解決了傳統(tǒng)機(jī)械臂不能在復(fù)雜環(huán)境下進(jìn)行探索的問題。所述繩驅(qū)蛇形機(jī)械臂包括n個(gè)錐節(jié)和關(guān)節(jié)組成，所述n大于3，所述方法包括：根據(jù)臂體末端點(diǎn)空間位置、末端點(diǎn)下一時(shí)刻空間位置及末端關(guān)節(jié)空間位置獲取下一時(shí)刻臂體末端關(guān)節(jié)空間位置和角度；根據(jù)下一時(shí)刻臂體末端關(guān)節(jié)空間位置和關(guān)節(jié)跟隨運(yùn)動(dòng)規(guī)劃條件獲取第i個(gè)關(guān)節(jié)的下一時(shí)刻位置及角度，直至獲得第2關(guān)節(jié)下一時(shí)刻空間位置和角度；根據(jù)第2個(gè)關(guān)節(jié)空間位置及角度獲取第1個(gè)關(guān)節(jié)空間位置及角度；獲取所有關(guān)節(jié)的角度，完成繩驅(qū)蛇形機(jī)械臂路徑規(guī)劃運(yùn)動(dòng)跟隨。適用于狹窄復(fù)雜的空間環(huán)境的路徑規(guī)劃。

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

本發(fā)明屬于機(jī)械臂控制技術(shù)領(lǐng)域，尤其涉及繩驅(qū)蛇形機(jī)械臂路徑規(guī)劃運(yùn)動(dòng)跟隨技術(shù)領(lǐng)域。

背景技術(shù)

在實(shí)際的工程應(yīng)用中存在大量狹窄復(fù)雜的環(huán)境需要被探測(cè)，如空間站艙室、飛機(jī)油箱、建筑物管道等。此類環(huán)境的探測(cè)一般要求不拆除原有結(jié)構(gòu)下進(jìn)行。若采用人工方法則檢測(cè)成本較高，且人員長(zhǎng)期在此類特殊環(huán)境工作也存在一定安全隱患。因此宜采用機(jī)械臂的等現(xiàn)代化機(jī)械設(shè)備進(jìn)行探測(cè)。傳統(tǒng)機(jī)械臂結(jié)構(gòu)較為簡(jiǎn)單、運(yùn)動(dòng)自由度有限，難以滿足復(fù)雜環(huán)境的探測(cè)需求。

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

本發(fā)明解決了傳統(tǒng)機(jī)械臂不能在復(fù)雜環(huán)境下進(jìn)行探索的問題。

本發(fā)明提供一種繩驅(qū)蛇形機(jī)械臂路徑規(guī)劃首尾運(yùn)動(dòng)跟隨方法，所述繩驅(qū)蛇形機(jī)械臂包括n個(gè)錐節(jié)和n個(gè)關(guān)節(jié)，所述n大于3，所述規(guī)劃運(yùn)動(dòng)跟隨方法包括：

根據(jù)繩驅(qū)蛇形機(jī)械臂臂體末端點(diǎn)的空間位置末端點(diǎn)下一時(shí)刻的空間位置及末端關(guān)節(jié)的空間位置獲取下一時(shí)刻臂體末端關(guān)節(jié)的空間位置和角度

根據(jù)下一時(shí)刻臂體末端關(guān)節(jié)的空間位置和關(guān)節(jié)跟隨運(yùn)動(dòng)規(guī)劃條件獲取第i個(gè)關(guān)節(jié)的下一時(shí)刻位置及角度，i＝n-1,n-2,……,2，依次類推，直至獲得第2關(guān)節(jié)下一時(shí)刻的空間位置和角度

根據(jù)第2個(gè)關(guān)節(jié)的空間位置及角度獲取第1個(gè)關(guān)節(jié)的空間位置及角度

根據(jù)獲取的所有關(guān)節(jié)的角度完成繩驅(qū)蛇形機(jī)械臂路徑規(guī)劃運(yùn)動(dòng)跟隨。

進(jìn)一步，所述獲取下一時(shí)刻臂體末端關(guān)節(jié)的空間位置和角度具體為：

臂體末端的第n個(gè)關(guān)節(jié)沿上一時(shí)刻t該關(guān)節(jié)的軸向方向運(yùn)動(dòng)，所述運(yùn)動(dòng)的時(shí)間步長(zhǎng)為Δt，該關(guān)節(jié)在下一時(shí)刻t+Δt的空間位置到末端距離為第n個(gè)錐節(jié)的長(zhǎng)度l_n；則根據(jù)求解方程：

獲得臂體末端的第n個(gè)關(guān)節(jié)的空間位置

臂體末端的第n個(gè)關(guān)節(jié)的角度根據(jù)求解方程獲得：

為臂體末端在慣性系下的x、y、z方向的坐標(biāo)。

進(jìn)一步，所述運(yùn)動(dòng)跟隨方法還包括判斷臂體末端的第n個(gè)關(guān)節(jié)的角度是否在臂體末端的關(guān)節(jié)的機(jī)構(gòu)運(yùn)動(dòng)空間內(nèi)：若所述關(guān)節(jié)角度在機(jī)構(gòu)運(yùn)動(dòng)空間內(nèi)，則空間位置為和為下一時(shí)刻臂體末端的第n個(gè)關(guān)節(jié)的空間位置和角度；否則，則判定繩驅(qū)蛇形機(jī)械臂在進(jìn)行平移運(yùn)動(dòng)，并獲取當(dāng)前繩驅(qū)蛇形機(jī)械臂末端第n個(gè)關(guān)節(jié)的空間位置和角度。

進(jìn)一步，所述獲取當(dāng)前繩驅(qū)蛇形機(jī)械臂末端第n個(gè)關(guān)節(jié)的空間位置和角度是根據(jù)求解方程獲得：

進(jìn)一步，所述根據(jù)下一時(shí)刻臂體末端關(guān)節(jié)的空間位置和關(guān)節(jié)跟隨運(yùn)動(dòng)規(guī)劃條件獲取第i個(gè)關(guān)節(jié)的下一時(shí)刻位置及角度，i＝n-1,n-2,……,2，依次類推，直至獲得第2關(guān)節(jié)下一時(shí)刻的空間位置和角度具體為：

獲取第i個(gè)關(guān)節(jié)的下一時(shí)刻位置根據(jù)求解方式：

所述i＝n-1,n-2,……,2；l_i為第i個(gè)錐節(jié)的長(zhǎng)度；