[發(fā)明專(zhuān)利]一種基于機(jī)器人動(dòng)力學(xué)模型的關(guān)節(jié)力矩限制方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710209778.7
• 申請(qǐng)日: 2017-03-31
• 公開(kāi)（公告）號(hào): CN106903692A
• 公開(kāi)（公告）日: 2017-06-30
• 發(fā)明（設(shè)計(jì)）人: 張鐵;羅欣;鄒焱飚
• 申請(qǐng)（專(zhuān)利權(quán)）人: 華南理工大學(xué)
• 主分類(lèi)號(hào): B25J9/16
• 分類(lèi)號(hào): B25J9/16
• 代理公司: 廣州粵高專(zhuān)利商標(biāo)代理有限公司44102
• 代理人: 何淑珍

1.一種基于機(jī)器人動(dòng)力學(xué)模型的關(guān)節(jié)力矩限制方法，其特征在于，包括如下步驟：

S1、設(shè)計(jì)一個(gè)關(guān)于時(shí)間的動(dòng)態(tài)標(biāo)度；

S2、建立期望關(guān)節(jié)力矩和限制關(guān)節(jié)力矩之間的函數(shù)關(guān)系；

S3、在機(jī)器人運(yùn)行過(guò)程中，實(shí)時(shí)計(jì)算限制力矩和期望力矩；

S4、如果期望力矩小于或者等于限制力矩，則返回上一步S3；如果期望力矩大于限制力矩，則進(jìn)行下一步S5；

S5、計(jì)算期望關(guān)節(jié)力矩和限制關(guān)節(jié)力矩的函數(shù)，求得相應(yīng)的時(shí)間動(dòng)態(tài)標(biāo)度，作為關(guān)節(jié)插補(bǔ)周期，以增加關(guān)節(jié)運(yùn)動(dòng)時(shí)間的方式，減小關(guān)節(jié)力矩。

2.根據(jù)權(quán)利要求1所述的一種基于機(jī)器人動(dòng)力學(xué)模型的關(guān)節(jié)力矩限制方法，其特征在于，所述步驟S1包含如下步驟：

S1.1、經(jīng)過(guò)軌跡規(guī)劃之后，機(jī)器人所有關(guān)節(jié)的運(yùn)動(dòng)軌跡為θ(t),t∈[0,t_f]，假定該關(guān)節(jié)運(yùn)動(dòng)軌跡某處存在關(guān)節(jié)力矩超限的情況，于是設(shè)計(jì)一條新的關(guān)節(jié)運(yùn)動(dòng)軌跡假定其能保證關(guān)節(jié)力矩保證安全限制范圍之內(nèi)，且滿(mǎn)足式(1)

$\widetilde{\mathrm{\θ}}\left(t\right)=\mathrm{\θ}\left(r\right)---\left(1\right)$

其中r＝r(t)，是關(guān)于時(shí)間t的嚴(yán)格單調(diào)遞增函數(shù)，且有r(0)＝0,r(t_f)＝t_f；

S1.2、對(duì)式(1)分別求一階和二階微分，可得

$\stackrel{\·}{\widetilde{\mathrm{\θ}}}\left(t\right)=\stackrel{\·}{\mathrm{\θ}}\left(r\right)\stackrel{\·}{r}\left(t\right)---\left(2\right)$

$\stackrel{\·\·}{\widetilde{\mathrm{\θ}}}\left(t\right)=\stackrel{\·\·}{\mathrm{\θ}}\left(r\right){\stackrel{\·}{r}}^2\left(t\right)+\stackrel{\·}{\mathrm{\θ}}\left(r\right)\stackrel{\·\·}{r}\left(t\right)---\left(3\right)$

對(duì)于軌跡θ(t)，機(jī)器人的逆動(dòng)力學(xué)模型如式(4)所示

$\mathrm{\τ}\left(t\right)=H\left(\mathrm{\θ}\right(t\left)\right)\stackrel{\·\·}{\mathrm{\θ}}\left(t\right)+C\left(\mathrm{\θ}\right(t),\stackrel{\·}{\mathrm{\θ}}(t\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(t\right)+g\left(\mathrm{\θ}\right(t\left)\right)+f\left(\stackrel{\·}{\mathrm{\θ}}\right(t\left)\right)---\left(4\right)$

將科氏力和向心力項(xiàng)重寫(xiě)為將重力項(xiàng)和摩擦力項(xiàng)獨(dú)立出來(lái)，可得式(4)

$\mathrm{\τ}\left(t\right)={\mathrm{\τ}}_a\left(t\right)+g\left(\mathrm{\θ}\right(t\left)\right)+f\left(\stackrel{\·}{\mathrm{\θ}}\right(t\left)\right)---\left(5\right)$

其中，

同理，對(duì)于軌跡可得

$\widetilde{\mathrm{\τ}}\left(t\right)={\widetilde{\mathrm{\τ}}}_a\left(t\right)+g\left(\widetilde{\mathrm{\θ}}\right(t\left)\right)+f\left(\stackrel{\·}{\widetilde{\mathrm{\θ}}}\right(t\left)\right)---\left(6\right)$

其中，

S1.3、將式(2)和式(3)代入整理得

${\widetilde{\mathrm{\τ}}}_a\left(t\right)=\[H\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·\·}{\mathrm{\θ}}\left(r\right)+\stackrel{\·}{\mathrm{\θ}}\left(r\right)C\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(r\right)\]{\stackrel{\·}{r}}^2\left(t\right)+H\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(r\right)\stackrel{\·\·}{r}\left(t\right)---\left(7\right)$

令t＝r(t)，將代入式(7)，得

${\widetilde{\mathrm{\τ}}}_a\left(t\right)={\stackrel{\·}{r}}^2\left(t\right){\mathrm{\τ}}_a\left(r\right)+\stackrel{\·\·}{r}\left(t\right)H\left(\mathrm{\θ}\right(r\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(r\right)---\left(8\right)$

綜上可得超限關(guān)節(jié)力矩τ(t)和不超限關(guān)節(jié)力矩與函數(shù)r(t)之間的關(guān)系，r(t)被稱(chēng)作動(dòng)態(tài)標(biāo)度函數(shù)，可以視作時(shí)間t的一種映射關(guān)系，相當(dāng)于重新調(diào)整時(shí)間的流速，將恒定不變流逝的時(shí)間，映射為可以發(fā)生變動(dòng)的函數(shù)關(guān)系；

S1.4、為了進(jìn)一步簡(jiǎn)化式(8)，動(dòng)態(tài)標(biāo)度函數(shù)r(t)可以選取如式(9)所示的最簡(jiǎn)單的線(xiàn)性標(biāo)度函數(shù)

r(t)＝ct (9)

式中，c為常數(shù)，則有

于是，式(5-9)可以簡(jiǎn)化為

${\widetilde{\mathrm{\τ}}}_a\left(t\right)=c^2{\mathrm{\τ}}_a\left(r\right).---\left(10\right)$

3.根據(jù)權(quán)利要求1所述的一種基于機(jī)器人動(dòng)力學(xué)模型的關(guān)節(jié)力矩限制方法，其特征在于，所述步驟S3包含如下步驟：

S3.1、t時(shí)刻，不超限關(guān)節(jié)力矩的慣性力、科氏力和向心力項(xiàng)可以求得

${\widetilde{\mathrm{\τ}}}_a\left(t\right)={\mathrm{\τ}}_{max}-g\left(\mathrm{\θ}\right(t\left)\right)-f\left(\stackrel{\·}{\mathrm{\θ}}\right(t\left)\right)---\left(11\right)$

式中，τ_max是關(guān)節(jié)限制力矩，一般為固定常數(shù)；g(θ(t))為機(jī)器人逆動(dòng)力學(xué)模型的重力項(xiàng)；為機(jī)器人逆動(dòng)力學(xué)模型的摩擦力項(xiàng)；

S3.2、t時(shí)刻，軌跡規(guī)劃的關(guān)節(jié)力矩的慣性力、科氏力和向心力項(xiàng)可以由機(jī)器人逆動(dòng)力學(xué)模型求得

${\mathrm{\τ}}_a\left(t\right)=H\left(\mathrm{\θ}\right(t\left)\right)\stackrel{\·\·}{\mathrm{\θ}}\left(t\right)+\stackrel{\·}{\mathrm{\θ}}\left(t\right)C\left(\mathrm{\θ}\right(t\left)\right)\stackrel{\·}{\mathrm{\θ}}\left(t\right)---\left(12\right)$

式中，H(θ(t))為機(jī)器人逆動(dòng)力學(xué)模型的慣性力項(xiàng)；為機(jī)器人逆動(dòng)力學(xué)模型的哥氏力和向心力項(xiàng)。