[發(fā)明專利]一種工業(yè)機械臂末端負(fù)載動力學(xué)參數(shù)估計方法

在審

• 申請?zhí)枺?/td>: 202110030103.2
• 申請日: 2021-01-11
• 公開（公告）號: CN112883504A
• 公開（公告）日: 2021-06-01
• 發(fā)明（設(shè)計）人: 郭發(fā)勇;周皞;芮夢霞;祝驊;鈕鑫;翟雅慧
• 申請（專利權(quán)）人: 常州工程職業(yè)技術(shù)學(xué)院
• 主分類號: G06F30/17
• 分類號: G06F30/17;G06F30/20;G06F119/14
• 代理公司: 南京知識律師事務(wù)所 32207
• 代理人: 高桂珍

1.一種工業(yè)機械臂末端負(fù)載動力學(xué)參數(shù)估計方法，其特征在于：包括如下步驟：

步驟1)：通過牛頓-歐拉迭代方法構(gòu)建工業(yè)機械臂本體動力學(xué)模型；步驟2)：通過多靜態(tài)點負(fù)載模型構(gòu)建工業(yè)機械臂負(fù)載參數(shù)辨識方程；步驟3)：求解得到不同負(fù)載的力學(xué)參數(shù)；所述步驟3)具體包括：利用多靜態(tài)點采集關(guān)節(jié)力矩數(shù)據(jù)，采樣數(shù)據(jù)通過低通濾波處理后帶入負(fù)載辨識方程，通過最小二乘法擬合形成線性方程組以求解。

2.根據(jù)權(quán)利要求1所述的一種工業(yè)機械臂末端負(fù)載動力學(xué)參數(shù)估計方法，其特征在于，所述步驟1)具體包括如下：

根據(jù)機械臂動力學(xué)可得：其中，τ_m為機器人關(guān)節(jié)扭矩輸出、τ_ext為人為拖動力、τ_f為摩擦總力，q、分別為機械臂的關(guān)節(jié)位置、速度和加速度，為機械臂慣性力，為耦合力，G(q)為重力，當(dāng)機械臂運動速度處于較低狀態(tài)時，即和非常小，計算說明此時項非常小，故機械臂各關(guān)節(jié)輸出扭矩可表示為：

此處和分別為機械臂重力和關(guān)節(jié)摩擦力的估計值。

3.根據(jù)權(quán)利要求1所述的一種工業(yè)機械臂末端負(fù)載動力學(xué)參數(shù)估計方法，其特征在于，所述步驟2)具體包括如下：

利用多靜態(tài)點估計工業(yè)機械臂末端負(fù)載參數(shù)辨識方法，其中，建立工業(yè)機械臂末端負(fù)載動力學(xué)參數(shù)模型，如下：

根據(jù)機械臂動力學(xué)可得：其中τ_m為機器人關(guān)節(jié)扭矩輸出、τ_f為摩擦總力，q、分別為機械臂的關(guān)節(jié)位置、速度和加速度，為機械臂慣性力，為耦合力，G(q)為重力，當(dāng)機器人處于靜止?fàn)顟B(tài)時，和均為零，計算說明此時項為零，摩擦力與關(guān)節(jié)運動速度有關(guān)，τ_f項也為零，故機械臂各關(guān)節(jié)輸出扭矩可表示為：

此處為機械臂重力的估計值，以KD協(xié)作機械臂的運動模型為基礎(chǔ)，直接得到此時的顯式方程，得到如下6個方程：

t1＝0；

t2＝(M5*g*x5*cos(q2+q3+q4+q5))/2+(M6*g*x6*cos(q2+q3+q4+q6))/2+M4*a4*g*cos(q2+q3)+M5*a4*g*cos(q2+q3)+M6*a4*g*cos(q2+q3)-(M5*g*y5*sin(q2+q3+q4+q5))/2-(M6*g*y6*sin(q2+q3+q4+q6))/2+(M6*g*z6*sin(q2+q3+q4+q5))/2+M3*g*x3*cos(q2+q3)-M3*g*y3*sin(q2+q3)+(M6*g*x6*cos(q2+q3+q4-q5-q6))/4+M3*a3*g*cos(q2)+M4*a3*g*cos(q2)+M5*a3*g*cos(q2)+M6*a3*g*cos(q2)+(M6*g*y6*sin(q2+q3+q4-q5-q6))/4+M2*g*x2*cos(q2)-M2*g*y2*sin(q2)+(M5*g*x5*cos(q2+q3+q4-q5))/2-(M6*g*x6*cos(q2+q3+q4-q6))/2+(M5*g*y5*sin(q2+q3+q4-q5))/2-(M6*g*y6*sin(q2+q3+q4-q6))/2+(M6*g*x6*cos(q2+q3+q4+q5+q6))/4-(M6*g*z6*sin(q2+q3+q4-q5))/2-(M6*g*y6*sin(q2+q3+q4+q5+q6))/4-M5*d5*g*sin(q2+q3+q4)-M6*d5*g*sin(q2+q3+q4)+M4*g*x4*cos(q2+q3+q4)-M4*g*y4*sin(q2+q3+q4)-M5*g*z5*sin(q2+q3+q4)+(M6*g*x6*cos(q2+q3+q4+q5-q6))/4+(M6*g*x6*cos(q2+q3+q4-q5+q6))/4+(M6*g*y6*sin(q2+q3+q4+q5-q6))/4-(M6*g*y6*sin(q2+q3+q4-q5+q6))/4；

t3＝M3*g*x3*cos(q2+q3)-M3*g*y3*sin(q2+q3)-M5*d5*g*sin(q2+q3+q4)-M6*d5*g*sin(q2+q3+q4)+M4*g*x4*cos(q2+q3+q4)-M4*g*y4*sin(q2+q3+q4)-M5*g*z5*sin(q2+q3+q4)+M4*a4*g*cos(q2+q3+q4)*cos(q4)+M5*a4*g*cos(q2+q3+q4)*cos(q4)+M6*a4*g*cos(q2+q3+q4)*cos(q4)+M5*g*x5*cos(q2+q3+q4)*cos(q5)+M4*a4*g*sin(q2+q3+q4)*sin(q4)+M5*a4*g*sin(q2+q3+q4)*sin(q4)+M6*a4*g*sin(q2+q3+q4)*sin(q4)-M5*g*y5*cos(q2+q3+q4)*sin(q5)-M6*g*y6*sin(q2+q3+q4)*cos(q6)+M6*g*z6*cos(q2+q3+q4)*sin(q5)-M6*g*x6*sin(q2+q3+q4)*sin(q6)+M6*d5*g*sin(q2+q3+q4)*cos(q5)^2-(M6*g*y6*sin(q2+q3+q4)*cos(q5+q6)*cos(q5))/2-(M6*g*x6*sin(q2+q3+q4)*sin(q5+q6)*cos(q5))/2+M6*g*x6*cos(q2+q3+q4)*cos(q5)*cos(q6)-M6*g*y6*cos(q2+q3+q4)*cos(q5)*sin(q6)-(M6*g*y6*cos(q5-q6)*sin(q2+q3+q4)*cos(q5))/2+(M6*g*x6*sin(q5-q6)*sin(q2+q3+q4)*cos(q5))/2-M6*a4*g*sin(q2+q3+q4)*cos(q5)^2*sin(q4)+M6*g*y6*sin(q2+q3+q4)*cos(q5)^2*cos(q6)+M6*g*x6*sin(q2+q3+q4)*cos(q5)^2*sin(q6)-(M6*d5*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q5)*sin(q6))/2+(M6*g*z6*sin(q2+q3+q4)*cos(q5+q6)*sin(q5)*sin(q6))/2-(M6*g*z6*sin(q2+q3+q4)*sin(q5+q6)*cos(q6)*sin(q5))/2-(M6*d5*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q5)*cos(q6))/2+(M6*a4*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q4)*sin(q6))/2+(M6*a4*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q4)*cos(q6))/2+(M6*d5*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q5)*sin(q6))/2+(M6*g*z6*cos(q5-q6)*sin(q2+q3+q4)*sin(q5)*sin(q6))/2+(M6*g*z6*sin(q5-q6)*sin(q2+q3+q4)*cos(q6)*sin(q5))/2-(M6*d5*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q5)*cos(q6))/2+(M6*a4*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q4)*sin(q6))/2-(M6*a4*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q4)*cos(q6))/2+(M6*a4*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q5)*cos(q6)*sin(q4))/2-(M6*a4*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q5)*sin(q4)*sin(q6))/2+(M6*a4*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q5)*cos(q6)*sin(q4))/2+(M6*a4*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q5)*sin(q4)*sin(q6))/2；

t4＝M4*g*x4*cos(q2+q3+q4)-M6*d5*g*sin(q2+q3+q4)-M5*d5*g*sin(q2+q3+q4)-M4*g*y4*sin(q2+q3+q4)-M5*g*z5*sin(q2+q3+q4)+M5*g*x5*cos(q2+q3+q4)*cos(q5)-M5*g*y5*cos(q2+q3+q4)*sin(q5)-M6*g*y6*sin(q2+q3+q4)*cos(q6)+M6*g*z6*cos(q2+q3+q4)*sin(q5)-M6*g*x6*sin(q2+q3+q4)*sin(q6)+M6*d5*g*sin(q2+q3+q4)*cos(q5)^2-(M6*g*y6*sin(q2+q3+q4)*cos(q5+q6)*cos(q5))/2-(M6*g*x6*sin(q2+q3+q4)*sin(q5+q6)*cos(q5))/2+M6*g*x6*cos(q2+q3+q4)*cos(q5)*cos(q6)-M6*g*y6*cos(q2+q3+q4)*cos(q5)*sin(q6)-(M6*g*y6*cos(q5-q6)*sin(q2+q3+q4)*cos(q5))/2+(M6*g*x6*sin(q5-q6)*sin(q2+q3+q4)*cos(q5))/2+M6*g*y6*sin(q2+q3+q4)*cos(q5)^2*cos(q6)+M6*g*x6*sin(q2+q3+q4)*cos(q5)^2*sin(q6)-(M6*d5*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q5)*sin(q6))/2+(M6*g*z6*sin(q2+q3+q4)*cos(q5+q6)*sin(q5)*sin(q6))/2-(M6*g*z6*sin(q2+q3+q4)*sin(q5+q6)*cos(q6)*sin(q5))/2-(M6*d5*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q5)*cos(q6))/2+(M6*d5*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q5)*sin(q6))/2+(M6*g*z6*cos(q5-q6)*sin(q2+q3+q4)*sin(q5)*sin(q6))/2+(M6*g*z6*sin(q5-q6)*sin(q2+q3+q4)*cos(q6)*sin(q5))/2-(M6*d5*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q5)*cos(q6))/2；

t5＝-(g*sin(q2+q3+q4)*(2*M5*x5*sin(q5)-M6*y6*cos(q5-q6)+M6*x6*sin(q5-q6)+M6*y6*cos(q5+q6)+M6*x6*sin(q5+q6)+2*M5*y5*cos(q5)-2*M6*z6*cos(q5)))/2；

t6＝M6*x6*((g*sin(q5-q6)*sin(q2+q3+q4))/2-(g*sin(q2+q3+q4)*sin(q5+q6))/2+g*cos(q2+q3+q4)*cos(q6))-M6*y6*((g*sin(q2+q3+q4)*cos(q5+q6))/2+g*cos(q2+q3+q4)*sin(q6)+(g*cos(q5-q6)*sin(q2+q3+q4))/2)。