[發(fā)明專利]一種基于關(guān)節(jié)性能限制的機(jī)械臂運(yùn)動(dòng)時(shí)間優(yōu)化方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202111366701.3
• 申請(qǐng)日: 2021-11-18
• 公開(kāi)（公告）號(hào): CN113787525B
• 公開(kāi)（公告）日: 2022-02-01
• 發(fā)明（設(shè)計(jì)）人: 劉家駿;雷成林
• 申請(qǐng)（專利權(quán)）人: 季華實(shí)驗(yàn)室
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 佛山市君創(chuàng)知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 44675
• 代理人: 王瑞長(zhǎng)

1.一種基于關(guān)節(jié)性能限制的機(jī)械臂運(yùn)動(dòng)時(shí)間優(yōu)化方法，其特征在于，包括以下步驟：

（1）機(jī)械臂的各個(gè)關(guān)節(jié)電機(jī)之間存在耦合關(guān)系，可通過(guò)Lagrange方程對(duì)此進(jìn)行描述，其中Lagrange方程為：

式中，M(θ)為6×6的關(guān)節(jié)空間慣量矩陣，為6×6的科氏力和向心力的耦合矩陣，G(θ)為6×1的重力矢量矩陣，θ為6×1的機(jī)械臂關(guān)節(jié)空間的旋轉(zhuǎn)角度矩陣，τ為6×1的機(jī)械臂的關(guān)節(jié)力矩矩陣；為對(duì)θ求導(dǎo)，即機(jī)械臂關(guān)節(jié)的速度矩陣；為對(duì)θ求兩次導(dǎo)，即機(jī)械臂關(guān)節(jié)的加速度矩陣；

利用Christoffel符號(hào)將上述方程可以改寫為以下動(dòng)力學(xué)運(yùn)動(dòng)方程：

式中，為機(jī)械臂關(guān)節(jié)的速度矩陣的轉(zhuǎn)置矩陣，為6×6×6的三維張量；

通過(guò)構(gòu)造映射的方式，將Lagrange方程映射到平面，從而得出：

式中，s為沿軌跡運(yùn)動(dòng)的軌跡長(zhǎng)度，為機(jī)械臂的末端執(zhí)行器沿軌跡運(yùn)動(dòng)的速度，為機(jī)械臂的末端執(zhí)行器沿軌跡運(yùn)動(dòng)的加速度，θ_s為關(guān)節(jié)角度對(duì)軌跡長(zhǎng)度的微分矩陣，θ_ss為關(guān)節(jié)角度對(duì)軌跡長(zhǎng)度的二階微分矩陣；

（2）對(duì)機(jī)械臂關(guān)節(jié)電機(jī)的力矩進(jìn)行限制，具體為：

式中，i為機(jī)械臂的第i個(gè)關(guān)節(jié)，τ_i為機(jī)械臂的第i個(gè)關(guān)節(jié)電機(jī)某時(shí)刻的力矩，τ_i為τ矩陣中的其中一個(gè)數(shù)值；τ_i^min為第i個(gè)關(guān)節(jié)電機(jī)的最小力矩，τ_i^max為第i個(gè)關(guān)節(jié)電機(jī)的最大力矩；

根據(jù)上述方程公式（1）、（2）、（3）和（4），得到在平面上基于機(jī)械臂關(guān)節(jié)電機(jī)力矩限制的約束不等式：

其中，

式中，為通過(guò)軌跡長(zhǎng)度映射出機(jī)械臂關(guān)節(jié)的角度值，具體為一個(gè)映射關(guān)系函數(shù)；

（3）對(duì)軌跡點(diǎn)位進(jìn)行加速度求解；先獲取在該軌跡點(diǎn)位上的每個(gè)機(jī)械臂關(guān)節(jié)能夠提供的末端執(zhí)行器加速度范圍，并在該軌跡點(diǎn)位上的每個(gè)機(jī)械臂關(guān)節(jié)能夠提供的末端執(zhí)行器加速度范圍的交集中取最大值作為該軌跡點(diǎn)位的加速度限制值，記為sdd；其中，對(duì)約束不等式（5）中的符號(hào)進(jìn)行變換：

從而將約束不等式轉(zhuǎn)化為：

其中，根據(jù)運(yùn)算規(guī)則，其中的a(s)、b(s)和c(s)均為12×1的矩陣；

根據(jù)約束不等式（6），軌跡點(diǎn)位上的機(jī)械臂每個(gè)關(guān)節(jié)能夠提供的末端執(zhí)行器加速度上限值和末端執(zhí)行器加速度下限值的獲取如下：

當(dāng)a_i(s)＞0時(shí)，機(jī)械臂關(guān)節(jié)能提供的末端執(zhí)行器加速度上限值為：

當(dāng)a_i(s)＜0時(shí)，機(jī)械臂關(guān)節(jié)能提供的末端執(zhí)行器加速度下限值為：

當(dāng)a_i(s)=0時(shí)，選取該軌跡點(diǎn)位前后相鄰的兩個(gè)軌跡點(diǎn)位中的較小的末端執(zhí)行器加速度限制值作為該軌跡點(diǎn)位能夠提供的末端執(zhí)行器加速度限制值；

在公式（7）和公式（8）中，i為機(jī)械臂上的第i個(gè)關(guān)節(jié)；a_i(s)為12×1矩陣a(s)中與第i個(gè)關(guān)節(jié)所對(duì)應(yīng)的數(shù)值，b_i(s)為12×1矩陣b(s)中與第i個(gè)關(guān)節(jié)所對(duì)應(yīng)的數(shù)值，c_i(s)為12×1矩陣c(s)中與第i個(gè)關(guān)節(jié)所對(duì)應(yīng)的數(shù)值；

（4）將機(jī)械臂關(guān)節(jié)電機(jī)的速度限制映射為沿軌跡運(yùn)動(dòng)的末端執(zhí)行器速度限制，具體地：

式中，t為時(shí)間，為機(jī)械臂關(guān)節(jié)角度的變化量，dt為積分時(shí)間間隔，ds為軌跡長(zhǎng)度的變化量，為機(jī)械臂末端執(zhí)行器的速度限制，為機(jī)械臂關(guān)節(jié)電機(jī)的速度限制，上述公式（9）的后部分具體為由機(jī)械臂關(guān)節(jié)電機(jī)的速度限制轉(zhuǎn)換為機(jī)械臂末端執(zhí)行器的速度限制；

通過(guò)上述公式（9）的映射，得出沿指定運(yùn)動(dòng)軌跡的所有離散點(diǎn)上基于關(guān)節(jié)電機(jī)速度限制對(duì)應(yīng)的平面曲線；

（5）對(duì)各類特征軌跡點(diǎn)位的加速度限制值進(jìn)行前后數(shù)值積分處理，獲取基于關(guān)節(jié)電機(jī)力矩限制對(duì)應(yīng)的平面曲線；其中各類特征軌跡點(diǎn)位的加速度限制值通過(guò)步驟（3）獲取，所述各類特征軌跡點(diǎn)位包括起始點(diǎn)、終止點(diǎn)以及間斷點(diǎn)；具體地，積分公式如下：

式中，為軌跡點(diǎn)位向前的軌跡長(zhǎng)度值，為軌跡點(diǎn)位向前的速度值，為當(dāng)前軌跡點(diǎn)位處的軌跡長(zhǎng)度值，當(dāng)前軌跡點(diǎn)位處的機(jī)械臂末端執(zhí)行器的速度值，為軌跡點(diǎn)位向后的軌跡長(zhǎng)度值，為軌跡點(diǎn)位向后的速度值，dt為積分時(shí)間間隔，sdd為步驟（3）中求得的該軌跡點(diǎn)位的加速度限制值；軌跡點(diǎn)位的前向?yàn)檐壽E長(zhǎng)度從1往0的方向，軌跡點(diǎn)位的后向?yàn)檐壽E長(zhǎng)度從0往1的方向，由于機(jī)械臂的運(yùn)動(dòng)軌跡已經(jīng)確定，因此采用歸一方法可得出軌跡長(zhǎng)度s的范圍為0-1；

（6）將步驟（4）中獲得的基于關(guān)節(jié)電機(jī)速度限制對(duì)應(yīng)的平面曲線與步驟（5）中獲得的基于關(guān)節(jié)電機(jī)力矩限制對(duì)應(yīng)的平面曲線進(jìn)行結(jié)合；將基于關(guān)節(jié)電機(jī)力矩限制對(duì)應(yīng)的平面曲線中高于基于關(guān)節(jié)電機(jī)速度限制對(duì)應(yīng)的平面曲線的部分舍去，同時(shí)在同一點(diǎn)位上取較小的速度作為曲線速度值，從而得到同時(shí)滿足關(guān)節(jié)電機(jī)速度限制和關(guān)節(jié)電機(jī)力矩限制且連續(xù)的軌跡長(zhǎng)度-速度限制曲線；

（7）將步驟（6）中所得到的軌跡長(zhǎng)度-速度限制曲線作為依據(jù)條件，機(jī)械臂末端執(zhí)行器進(jìn)行指定運(yùn)動(dòng)軌跡的運(yùn)行，完成機(jī)械臂運(yùn)動(dòng)時(shí)間的優(yōu)化。