[發(fā)明專利]一種基于遞階結(jié)構(gòu)MPC的多自由度柔性關(guān)節(jié)機(jī)械臂系統(tǒng)的控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910036138.X
• 申請(qǐng)日: 2019-01-15
• 公開(kāi)（公告）號(hào): CN109648564B
• 公開(kāi)（公告）日: 2021-07-06
• 發(fā)明（設(shè)計(jì)）人: 黃向慧;倪琪;潘紅光;米文毓;黃心怡;蘇濤;溫帆
• 申請(qǐng)（專利權(quán)）人: 西安科技大學(xué)
• 主分類(lèi)號(hào): B25J9/16
• 分類(lèi)號(hào): B25J9/16
• 代理公司: 西安智大知識(shí)產(chǎn)權(quán)代理事務(wù)所 61215
• 代理人: 段俊濤

1.一種基于遞階結(jié)構(gòu)MPC的多自由度柔性關(guān)節(jié)機(jī)械臂系統(tǒng)的控制方法，其特征在于，包括如下步驟：

步驟一、假設(shè)連接在電機(jī)轉(zhuǎn)子與連桿之間的彈簧彈性系數(shù)為柔性關(guān)節(jié)的剛度系數(shù)，即關(guān)節(jié)無(wú)減速裝置，建立柔性關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)模型；

步驟二、設(shè)計(jì)整體結(jié)構(gòu)：首先在逆運(yùn)動(dòng)學(xué)中加入軌跡規(guī)劃，構(gòu)成笛卡爾控制器，其次將動(dòng)力學(xué)部分與運(yùn)動(dòng)學(xué)部分相互聯(lián)系，使得機(jī)械臂能夠穩(wěn)定運(yùn)行笛卡爾控制器所設(shè)計(jì)的目標(biāo)軌跡；

步驟三、控制器設(shè)計(jì)：在PD控制器的基礎(chǔ)上，設(shè)計(jì)一種遞階結(jié)構(gòu)的控制器，將上層預(yù)測(cè)控制器進(jìn)行多變量控制，得到每一個(gè)關(guān)節(jié)運(yùn)動(dòng)PD控制器的設(shè)定值，再送入下層PD控制器進(jìn)行實(shí)現(xiàn)；

步驟四、在所述整體結(jié)構(gòu)下，基于所述遞階結(jié)構(gòu)的控制器，實(shí)現(xiàn)多自由度柔性關(guān)節(jié)機(jī)械臂系統(tǒng)的控制。

2.根據(jù)權(quán)利要求1所述基于遞階結(jié)構(gòu)MPC的多自由度柔性關(guān)節(jié)機(jī)械臂系統(tǒng)的控制方法，其特征在于，所述柔性關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)模型的公式如下：

式中，H(q)為機(jī)械臂的慣量矩陣，為關(guān)節(jié)空間的科氏力與向心力系數(shù)矩陣，G(q)為重力向量；

其中h_1i、h_2i、h_3i分別為機(jī)械臂連桿1、連桿2、連桿3的慣量；c_1i、c_2i、c_3i分別為關(guān)節(jié)空間的關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3的科氏力與向心力系數(shù)；g₁、g₂、g₃分別為連桿1、連桿2、連桿3的重力向量；

J為轉(zhuǎn)子慣量矩陣，K為關(guān)節(jié)扭轉(zhuǎn)剛度矩陣，τ_m為電機(jī)驅(qū)動(dòng)力矩向量，θ為電機(jī)轉(zhuǎn)角向量，q為連桿轉(zhuǎn)角向量；

其中j₁、j₂、j₃分別為關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3的轉(zhuǎn)子慣量；k₁、k₂、k₃-分別為關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3的扭轉(zhuǎn)剛度系數(shù)；j₁-j₃、k₁-k₃是給定值，可直接從零件的產(chǎn)品手冊(cè)獲得；τ_m1、τ_m2、τ_m3分別為關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3的電機(jī)驅(qū)動(dòng)力矩；θ1、θ₂、θ₃分別為關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3的電機(jī)轉(zhuǎn)角，由傳感器測(cè)量得到；q₁、q₂、q₃分別為連桿1、連桿2、連桿3的轉(zhuǎn)角，由傳感器測(cè)量得到。

3.根據(jù)權(quán)利要求1所述基于遞階結(jié)構(gòu)MPC的多自由度柔性關(guān)節(jié)機(jī)械臂系統(tǒng)的控制方法，其特征在于，所述整體結(jié)構(gòu)設(shè)計(jì)過(guò)程如下：

1.將目標(biāo)軌跡即已經(jīng)規(guī)劃的軌跡輸入逆運(yùn)動(dòng)學(xué)中構(gòu)成笛卡爾控制器，并解算目標(biāo)關(guān)節(jié)角度，輸出到PD控制器中；

2.PD控制器將關(guān)節(jié)角度轉(zhuǎn)換成力矩驅(qū)動(dòng)關(guān)節(jié)層；

3.關(guān)節(jié)層動(dòng)作，由傳感器測(cè)得實(shí)際關(guān)節(jié)角度并輸出到正、逆運(yùn)動(dòng)學(xué)中；

4.實(shí)際關(guān)節(jié)角度輸入到正運(yùn)動(dòng)學(xué)中，得到實(shí)際機(jī)械臂軌跡；

5.實(shí)際關(guān)節(jié)角度反饋至逆運(yùn)動(dòng)學(xué)中，通過(guò)解算得到下一步的目標(biāo)關(guān)節(jié)角度；

6.返回步驟1，如此循環(huán)，直至機(jī)械臂執(zhí)行完目標(biāo)軌跡。

4.根據(jù)權(quán)利要求1所述基于遞階結(jié)構(gòu)MPC的多自由度柔性關(guān)節(jié)機(jī)械臂系統(tǒng)的控制方法，其特征在于，所述遞階結(jié)構(gòu)的控制器為多輸入多輸出的MPC控制器，MPC通過(guò)把約束加到未來(lái)輸入、輸出上，將約束顯式表示在二次規(guī)劃或非線性規(guī)劃問(wèn)題中，當(dāng)k≤1時(shí)，所搭建的柔性關(guān)節(jié)機(jī)械臂模型被用來(lái)預(yù)測(cè)未來(lái)輸出；在第k步，在線求解一個(gè)涉及未來(lái)時(shí)域即預(yù)測(cè)時(shí)域N_P內(nèi)預(yù)測(cè)信息的優(yōu)化問(wèn)題，來(lái)計(jì)算未來(lái)一段時(shí)域即控制時(shí)域N_C內(nèi)的控制輸入；在第k+1步，該優(yōu)化問(wèn)題所涉及的時(shí)域向前推移一步，再次求解；后續(xù)時(shí)刻依次滾動(dòng)進(jìn)行。

5.根據(jù)權(quán)利要求1所述基于遞階結(jié)構(gòu)MPC的多自由度柔性關(guān)節(jié)機(jī)械臂系統(tǒng)的控制方法，其特征在于，所述步驟四中，控制算法如下：

1.通過(guò)逆運(yùn)動(dòng)解算目標(biāo)軌跡得到目標(biāo)關(guān)節(jié)角度；

2.將目標(biāo)關(guān)節(jié)角度以矩陣形式輸入上層MPC控制器中，得到優(yōu)化后的關(guān)節(jié)角度；

3.將優(yōu)化后的關(guān)節(jié)角度輸入下層PD控制器中，驅(qū)動(dòng)關(guān)節(jié)，并通過(guò)傳感器測(cè)量實(shí)際關(guān)節(jié)角度；

4.將實(shí)際關(guān)節(jié)角度輸入到正運(yùn)動(dòng)學(xué)，得到實(shí)際機(jī)械臂運(yùn)動(dòng)軌跡；

5.同時(shí)將實(shí)際關(guān)節(jié)角度反饋上層MPC控制器中，進(jìn)行校正；

6.返回步驟1，如此循環(huán)，直至機(jī)械臂執(zhí)行完目標(biāo)軌跡。