[發(fā)明專利]一種冗余度機(jī)械臂突加度層運(yùn)動(dòng)規(guī)劃的初始化方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202111144929.8
• 申請(qǐng)日: 2021-09-28
• 公開（公告）號(hào): CN113771038A
• 公開（公告）日: 2021-12-10
• 發(fā)明（設(shè)計(jì)）人: 郭東生;李煊鮮;劉慶平;黎子豪;殷海波
• 申請(qǐng)（專利權(quán)）人: 千翼藍(lán)犀智能制造科技（廣州）有限公司
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16;B25J18/00
• 代理公司: 北京慕達(dá)星云知識(shí)產(chǎn)權(quán)代理事務(wù)所(特殊普通合伙) 11465
• 代理人: 符繼超

本發(fā)明公開了一種冗余度機(jī)械臂突加度層運(yùn)動(dòng)規(guī)劃的初始化方法，包括啟動(dòng)機(jī)械臂控制器，通過編碼器讀取機(jī)械臂各個(gè)關(guān)節(jié)當(dāng)前的角度；將各個(gè)關(guān)節(jié)當(dāng)前的角度作為起始值；將機(jī)械臂執(zhí)行運(yùn)動(dòng)規(guī)劃任務(wù)時(shí)的關(guān)節(jié)角度作為期望值；基于起始值和期望值，在突加度層上建立二次型優(yōu)化方案；將二次型優(yōu)化方案轉(zhuǎn)換為分段線性投影方程，并采用數(shù)值算法對(duì)分段線性投影方程進(jìn)行求解，得到最優(yōu)解；機(jī)械臂控制器根據(jù)最優(yōu)解，驅(qū)動(dòng)機(jī)械臂的各個(gè)關(guān)節(jié)，使其從當(dāng)前位置調(diào)整到機(jī)械臂執(zhí)行運(yùn)動(dòng)規(guī)劃任務(wù)時(shí)的位置；通過該方法能夠在突加度層上實(shí)現(xiàn)對(duì)冗余度機(jī)械臂的初始化工作，從而避免機(jī)械臂每次執(zhí)行運(yùn)動(dòng)規(guī)劃任務(wù)時(shí)都需要度量各個(gè)關(guān)節(jié)角度的繁瑣過程。

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

本發(fā)明屬于冗余度機(jī)械臂突加度層運(yùn)動(dòng)規(guī)劃領(lǐng)域，特別是一種冗余度機(jī)械臂突加度層運(yùn)動(dòng)規(guī)劃的初始化方法。

背景技術(shù)

冗余度機(jī)械臂是一種自身自由度多于執(zhí)行運(yùn)動(dòng)規(guī)劃任務(wù)時(shí)所需最少自由度的機(jī)械裝置；目前已廣泛應(yīng)用于焊接組裝、產(chǎn)品加工、機(jī)器作業(yè)等國(guó)民經(jīng)濟(jì)生產(chǎn)活動(dòng)中。冗余度機(jī)械臂在實(shí)際的運(yùn)動(dòng)規(guī)劃中最常見的一個(gè)問題是關(guān)節(jié)角度的初始化問題：每個(gè)運(yùn)動(dòng)規(guī)劃任務(wù)的執(zhí)行都需要指定一個(gè)初始的關(guān)節(jié)角度；如果機(jī)械臂的關(guān)節(jié)沒有精確到達(dá)其特定的初始位置，那么機(jī)械臂就無法成功地完成相應(yīng)的運(yùn)動(dòng)規(guī)劃任務(wù)。目前已有一些關(guān)節(jié)角度調(diào)整方案被提出并使得機(jī)械臂的關(guān)節(jié)能夠有效到達(dá)期望的位置。然而，這些方案都是在速度層和加速度層上進(jìn)行研究設(shè)計(jì)的，它們無法處理機(jī)械臂的關(guān)節(jié)突加度極限，也無法直接用于在突加度層上控制的機(jī)械臂，從而導(dǎo)致速度層或加速度層方案的適用性有限。

因此，如何在突加度層上實(shí)現(xiàn)冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃初始化，已經(jīng)成為當(dāng)前研究的關(guān)鍵問題。

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

鑒于上述問題，本發(fā)明提供一種至少解決上述部分技術(shù)問題的一種冗余度機(jī)械臂突加度層運(yùn)動(dòng)規(guī)劃的初始化方法，該方法能夠直接用于在突加度層上控制機(jī)械臂，且該方法操作方便、工作量少、作業(yè)規(guī)范。

本發(fā)明實(shí)施例提供了一種冗余度機(jī)械臂突加度層運(yùn)動(dòng)規(guī)劃的初始化方法，包括：

S1、啟動(dòng)機(jī)械臂控制器，通過編碼器讀取機(jī)械臂各個(gè)關(guān)節(jié)當(dāng)前的角度；

S2、將所述各個(gè)關(guān)節(jié)當(dāng)前的角度作為起始值；將機(jī)械臂執(zhí)行運(yùn)動(dòng)規(guī)劃任務(wù)時(shí)的關(guān)節(jié)角度作為期望值；基于所述起始值和期望值，在突加度層上建立二次型優(yōu)化方案；

S3、將所述二次型優(yōu)化方案轉(zhuǎn)換為分段線性投影方程，并采用數(shù)值算法對(duì)所述分段線性投影方程進(jìn)行求解，得到最優(yōu)解；

S4、所述機(jī)械臂控制器根據(jù)所述最優(yōu)解，驅(qū)動(dòng)機(jī)械臂的各個(gè)關(guān)節(jié)，使其從當(dāng)前位置調(diào)整到所述機(jī)械臂執(zhí)行運(yùn)動(dòng)規(guī)劃任務(wù)時(shí)的位置。

進(jìn)一步地，所述S2中二次型優(yōu)化方案的性能指標(biāo)表示為：

最小化

其中，θ∈Rⁿ表示機(jī)械臂關(guān)節(jié)角度；表示機(jī)械臂關(guān)節(jié)速度；表示機(jī)械臂關(guān)節(jié)加速度；表示機(jī)械臂關(guān)節(jié)突加度；θ_d∈Rⁿ表示機(jī)械臂執(zhí)行運(yùn)動(dòng)規(guī)劃任務(wù)時(shí)的起始關(guān)節(jié)角度；T表示矩陣或向量的轉(zhuǎn)置；參數(shù)λ＞0∈R。

進(jìn)一步地，所述二次型優(yōu)化方案的約束條件包括：

θ^-≤θ≤θ⁺ (2)

其中，θ^-表示機(jī)械臂關(guān)節(jié)角度的下限；θ⁺表示機(jī)械臂關(guān)節(jié)角度的上限；表示機(jī)械臂關(guān)節(jié)速度的下限；表示機(jī)械臂關(guān)節(jié)速度的上限；表示機(jī)械臂關(guān)節(jié)加速度的下限；表示機(jī)械臂關(guān)節(jié)加速度的上限；表示機(jī)械臂關(guān)節(jié)突加度的下限；表示機(jī)械臂關(guān)節(jié)突加度的上限。

進(jìn)一步地，將公式(2)-(5)轉(zhuǎn)化為關(guān)于機(jī)械臂關(guān)節(jié)突加度的雙端約束，表示為：