[發(fā)明專利]一種柔性機(jī)器人動(dòng)力學(xué)建模及軌跡跟隨控制方法及裝置

在審

• 申請?zhí)枺?/td>: 202110228437.0
• 申請日: 2021-03-02
• 公開（公告）號: CN113146617A
• 公開（公告）日: 2021-07-23
• 發(fā)明（設(shè)計(jì)）人: 彭鍵清;韓瑜;吳皓軒
• 申請（專利權(quán)）人: 中山大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 深圳市創(chuàng)富知識產(chǎn)權(quán)代理有限公司 44367
• 代理人: 高冰

本發(fā)明公開了一種柔性機(jī)器人動(dòng)力學(xué)建模及軌跡跟隨控制方法及裝置，根據(jù)“離散式剛性連桿+聯(lián)動(dòng)機(jī)構(gòu)”構(gòu)型繩索驅(qū)動(dòng)串聯(lián)機(jī)器人的機(jī)構(gòu)幾何參數(shù)，借助D?H參數(shù)法建立起關(guān)節(jié)?末端的運(yùn)動(dòng)學(xué)模型；再根據(jù)關(guān)節(jié)間繩索傳動(dòng)的關(guān)系，建立了“繩索?關(guān)節(jié)?末端”的多重運(yùn)動(dòng)學(xué)耦合模型。進(jìn)一步地，提出了分段聯(lián)動(dòng)式串?并聯(lián)混合的動(dòng)力學(xué)求解方法，利用主從被動(dòng)式的傳動(dòng)特點(diǎn)，通過對驅(qū)動(dòng)繩索與聯(lián)動(dòng)繩索作用力進(jìn)行求解，推導(dǎo)并建立了繩索驅(qū)動(dòng)串聯(lián)機(jī)器人的串?并聯(lián)混合的動(dòng)力學(xué)模型。并且將所得動(dòng)力學(xué)模型作為控制器的前饋控制輸入，補(bǔ)償運(yùn)動(dòng)誤差，同時(shí)設(shè)計(jì)非線性二次規(guī)劃模塊，對繩索拉力和操作空間精度指標(biāo)的進(jìn)行加權(quán)優(yōu)化，實(shí)現(xiàn)了較高精度的末端操作空間位姿的跟隨運(yùn)動(dòng)，并使得繩索拉力和末端位姿的同步優(yōu)化。

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

本發(fā)明涉及機(jī)器人控制領(lǐng)域，尤其是一種柔性機(jī)器人動(dòng)力學(xué)建模及軌跡跟隨控制方法及裝置。

技術(shù)背景

如今對智能機(jī)器人的環(huán)境適應(yīng)性以及環(huán)境限制的克服能力有著越來越高的要求，由于傳統(tǒng)工業(yè)機(jī)器人工作空間小、運(yùn)動(dòng)靈活性能力不足，特別是對一些非結(jié)構(gòu)化的環(huán)境適應(yīng)能力很弱，并且面臨自由度數(shù)限制以及剛性臂桿難以完成狹小環(huán)境中各類障礙物的穿越問題。繩驅(qū)超冗余機(jī)器人具有自由度數(shù)多、臂桿細(xì)小、運(yùn)動(dòng)靈活性強(qiáng)以及環(huán)境適應(yīng)性好、但剛度差等特點(diǎn)。采用離散式剛性連桿與分段聯(lián)動(dòng)構(gòu)型的繩驅(qū)超冗余機(jī)器人在保持靈活性的同時(shí)大大提升了機(jī)械臂的剛度，能滿足很多狹小空間的穿越任務(wù)，例如在核電站冷卻管道的維修、油氣管道維護(hù)以及核反應(yīng)堆管道檢查等超常規(guī)作業(yè)區(qū)域。

然而，由于該類型機(jī)構(gòu)自由度比較多，主從被動(dòng)式分段聯(lián)動(dòng)機(jī)械臂的多層級動(dòng)力學(xué)耦合的存在，使得此類機(jī)器人的動(dòng)力學(xué)控制與軌跡規(guī)劃變得更加復(fù)雜。因此需要提出一種能夠提高軌跡規(guī)劃效率的同時(shí)兼顧控制精度的柔性機(jī)器人軌跡規(guī)劃方法。

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

鑒于現(xiàn)有技術(shù)的缺陷，本發(fā)明旨在于提供一種柔性機(jī)器人動(dòng)力學(xué)建模及軌跡跟隨控制方法及裝置，能夠更好地研究機(jī)器人的運(yùn)動(dòng)特性并提高機(jī)器人末端軌跡跟隨的精確度。

為了實(shí)現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案如下：

一種柔性機(jī)器人動(dòng)力學(xué)建模及軌跡跟隨的控制裝置，具有柔性機(jī)器人，以及所述控制裝置，所述控制裝置包括：

偏差數(shù)據(jù)獲取模塊：用于獲取相對偏差數(shù)據(jù)并構(gòu)建所述柔性機(jī)器人空間模型；

計(jì)算模塊：用于實(shí)時(shí)計(jì)算期望軌跡的笛卡爾空間樣條插值、運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)以及求解非線性二次規(guī)劃優(yōu)化模型；

閾值判斷模塊：用于根據(jù)所述相對偏差數(shù)據(jù)與非線性二次規(guī)劃優(yōu)化模型的閾值條件，判斷末端位姿、關(guān)節(jié)角度、繩索拉力是否在設(shè)定目標(biāo)范圍內(nèi)；

執(zhí)行模塊：用于當(dāng)滿足閾值判斷條件，將非線性二次規(guī)劃優(yōu)化模型的計(jì)算結(jié)果輸入至PD控制器。控制器輸出信號到執(zhí)行模塊并作用到所控制柔性機(jī)器人；

其中，所述相對偏差數(shù)據(jù)包括所述柔性機(jī)器人末端點(diǎn)和設(shè)定的期望點(diǎn)的相對位位移與位姿的偏差、實(shí)際繩索長度和期望繩索長度的偏差以及繩索實(shí)際拉力與期望拉力的偏差。

本發(fā)明還提供一種實(shí)現(xiàn)柔性機(jī)器人動(dòng)力學(xué)建模及軌跡跟隨的控制的方法，所述方法包括以下步驟：

S1建立“繩索-關(guān)節(jié)-末端”多重傳動(dòng)運(yùn)動(dòng)學(xué)模型,對目標(biāo)柔性機(jī)器人各關(guān)節(jié)建立D-H坐標(biāo)系，推導(dǎo)得柔性機(jī)器人末端位姿、速度、加速度與關(guān)節(jié)角、角速度、角加速度之間的關(guān)系，建立起關(guān)節(jié)空間到末端笛卡爾空間的映射關(guān)系；

S2建立了串-并聯(lián)混合的動(dòng)力學(xué)模型：基于牛頓—?dú)W拉方程對柔性機(jī)器人整體慣性力、重力進(jìn)行求解；求解所述驅(qū)動(dòng)力與摩擦力；求解整個(gè)柔性機(jī)器人的聯(lián)動(dòng)繩索力/力矩；并進(jìn)一步地推導(dǎo)出柔性機(jī)器人串-并聯(lián)混合動(dòng)力學(xué)模型；

S3建立基于串-并聯(lián)混合的動(dòng)力學(xué)模型前饋的PD控制器：設(shè)計(jì)柔性機(jī)器人運(yùn)動(dòng)的PD控制器，并將步驟S2中的串-并聯(lián)混合的動(dòng)力學(xué)模型作為控制前饋輸入，補(bǔ)償柔性機(jī)器人末端的運(yùn)動(dòng)誤差；