本發(fā)明涉及串聯(lián)機(jī)器人逆動力學(xué)計算方法，特別涉及一種串聯(lián)機(jī)器人逆動力學(xué)快速計算方法。

背景技術(shù)：

串聯(lián)機(jī)器人在制造業(yè)、農(nóng)業(yè)、娛樂業(yè)等行業(yè)得到越來越廣泛的應(yīng)用，許多應(yīng)用場景需要串聯(lián)機(jī)器人擁有高速、高精度的運(yùn)動性能。基于串聯(lián)機(jī)器人動力學(xué)模型的運(yùn)動控制能賦予機(jī)器人高速、高精度的運(yùn)動性能，這種方法需要不斷實時進(jìn)行串聯(lián)機(jī)器人逆動力學(xué)計算。

目前串聯(lián)機(jī)器人逆動力學(xué)模型多以拉格朗日方程法進(jìn)行建模，這種方法得到的串聯(lián)機(jī)器人逆動力學(xué)模型十分復(fù)雜，以6自由度的串聯(lián)機(jī)器人為例，使用拉格朗日方程法得到的逆動力學(xué)模型，總共需要計算66271次乘法和51548次加法。而串聯(lián)機(jī)器人控制器的伺服控制周期一般為1ms，顯然如此復(fù)雜的模型無法在1ms之內(nèi)完成計算。因此，需要更簡潔的模型和更快速的計算方法才能實現(xiàn)串聯(lián)機(jī)器人逆動力學(xué)實時計算。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的是提供一種串聯(lián)機(jī)器人逆動力學(xué)快速計算方法，旨在解決串聯(lián)機(jī)器人逆動力學(xué)實時計算問題。

本發(fā)明的目的通過下述技術(shù)方案來實現(xiàn)：

一種串聯(lián)機(jī)器人逆動力學(xué)快速計算方法，包括如下步驟：

s1、使用牛頓-歐拉方程建立相對簡潔的串聯(lián)機(jī)器人逆動力學(xué)模型；

s2、解耦得到標(biāo)準(zhǔn)動力學(xué)參數(shù)和對應(yīng)的回歸矩陣；

s3、使用基于數(shù)值的簡化方法對解耦后模型進(jìn)行簡化，得到基本動力學(xué)參數(shù)和對應(yīng)的基本回歸矩陣；

s4、借助符號運(yùn)算工具對基本回歸矩陣進(jìn)行合并和簡化，并提取反復(fù)出現(xiàn)的項作為中間項，優(yōu)先計算以減少計算量。

進(jìn)一步地，所述步驟s2包含如下步驟：

s2.1、使用迭代牛頓-歐拉動力學(xué)算法得到串聯(lián)機(jī)器人的逆動力學(xué)表達(dá)式

式中，γ為各關(guān)節(jié)力矩；分別為各關(guān)節(jié)的轉(zhuǎn)角、角速度和角加速度；δ為各連桿的標(biāo)準(zhǔn)動力學(xué)參數(shù)，且δ＝[δ1δ2…δn]^t，δk為連桿k的標(biāo)準(zhǔn)動力學(xué)參數(shù)且δk＝[lxxklxyklxzklyyklyzklzzklxklyklzkmkfvkfckiak]^t；invdyn(·)為機(jī)器人逆動力學(xué)模型函數(shù)；

為了便于進(jìn)行串聯(lián)機(jī)器人動力學(xué)參數(shù)辨識，需要將式(1)改寫成如式(2)所示的線性形式

式中，被稱作回歸矩陣，是關(guān)于關(guān)節(jié)的轉(zhuǎn)角、角速度和角加速度的函數(shù)；

s2.2、依次令矢量δ的第i個動力學(xué)參數(shù)等于1，其余動力學(xué)參數(shù)等于0，然后代入式(1)所示的invdyn(·)函數(shù)中，則除去零項后，剩余項均為第i個動力學(xué)參數(shù)的相關(guān)項，將其賦值給回歸矩陣h的相關(guān)列，最終得到完整的回歸矩陣h。

進(jìn)一步地，所述步驟s3包含如下步驟：

s3.1、可以將矩陣和機(jī)器人標(biāo)準(zhǔn)動力學(xué)參數(shù)δ分成兩部分，則動力學(xué)方程可以寫成如下的形式

式中，hb為矩陣所有nb個線性無關(guān)列組成的子矩陣；hd為剩下的nd個全零列向量和線性相關(guān)列組成的子矩陣；δb為基本動力學(xué)參數(shù)；δd為對動力學(xué)不起作用的動力學(xué)參數(shù)；

hd可以用hb線性表示，寫作

hd＝hbkd；(4)

s3.2、設(shè)置換矩陣p＝[pbpd]滿足如下式

hp＝[hbhd]，(5)

則有

聯(lián)立式(3)到(6)可以得到

hb(δb+kdδd)＝γ，(7)

寫成含基本動力學(xué)參數(shù)項的動力學(xué)方程，則為

式中，β為基本動力學(xué)參數(shù)，β＝kδ，k＝pb^t+kdpd^t；

s3.3、隨機(jī)生成s組且滿足s遠(yuǎn)遠(yuǎn)大于機(jī)器人標(biāo)準(zhǔn)動力學(xué)參數(shù)δ的個數(shù)；將s組代入回歸矩陣函數(shù)可以得到回歸矩陣hs；對hs使用qr分解可以得到正則上三角矩陣r

遍歷矩陣r的對角元素，記錄其中非零對角元素在矩陣中的列號，依次寫入數(shù)組db；定義行列數(shù)等于機(jī)器人標(biāo)準(zhǔn)動力學(xué)參數(shù)δ個數(shù)的單位矩陣p'；依照數(shù)組db記錄的列號，按順序從單位矩陣p'中取出對應(yīng)的列組成新矩陣，該矩陣為pb，剩余的列組成新矩陣，該矩陣為pd，則p＝[pbpd]。

對矩陣hb,hd分別使用qr分解，可得

則有

聯(lián)立式(5)和式(10)，可以得到

kd＝rb^-1rd，(12)

綜上所述，可以得到pb,kd,pd，進(jìn)而得到基本動力學(xué)參數(shù)β。

本發(fā)明相對于現(xiàn)有技術(shù)具有如下的優(yōu)點及效果：

本發(fā)明采用牛頓-歐拉方程、簡化動力學(xué)參數(shù)和提取重復(fù)項三個步驟，大大減少串聯(lián)機(jī)器人逆動力學(xué)運(yùn)算量。以6自由度串聯(lián)機(jī)器人逆動力學(xué)模型為例，使用本發(fā)明的計算方法最終只需要計算551次乘法和312次加法，而市面常見的運(yùn)動控制系統(tǒng)基本都能在1ms內(nèi)完成1000次以內(nèi)的乘法和加法。因此，本發(fā)明能解決串聯(lián)機(jī)器人逆動力學(xué)運(yùn)算過于復(fù)雜，難以滿足實時計算要求的問題。

附圖說明

圖1是本發(fā)明實施例的串聯(lián)機(jī)器人逆動力學(xué)快速計算方法流程示意圖。

具體實施方式

下面結(jié)合實施例及附圖對本發(fā)明作進(jìn)一步的詳細(xì)描述，但本發(fā)明的實施方式不限于此。

如圖1所示，現(xiàn)以某六自由度垂直關(guān)節(jié)串聯(lián)機(jī)器人為對象，按照以下步驟進(jìn)行串聯(lián)機(jī)器人逆動力學(xué)快速計算：

一種串聯(lián)機(jī)器人逆動力學(xué)快速計算方法，包括如下步驟：

s1、使用牛頓-歐拉方程建立相對簡潔的串聯(lián)機(jī)器人逆動力學(xué)模型；

s2、解耦得到標(biāo)準(zhǔn)動力學(xué)參數(shù)和對應(yīng)的回歸矩陣；

s3、使用基于數(shù)值的簡化方法對解耦后模型進(jìn)行簡化，得到基本動力學(xué)參數(shù)和對應(yīng)的基本回歸矩陣；

s4、借助符號運(yùn)算工具對基本回歸矩陣進(jìn)行合并和簡化，并提取反復(fù)出現(xiàn)的項作為中間項，優(yōu)先計算以減少計算量。

具體而言，所述步驟s2包含如下步驟：

s2.1、使用迭代牛頓-歐拉動力學(xué)算法得到串聯(lián)機(jī)器人的逆動力學(xué)表達(dá)式

式中，γ為各關(guān)節(jié)力矩；分別為各關(guān)節(jié)的轉(zhuǎn)角、角速度和角加速度；δ為各連桿的標(biāo)準(zhǔn)動力學(xué)參數(shù)，且δ＝[δ1δ2…δn]^t，δk為連桿k的標(biāo)準(zhǔn)動力學(xué)參數(shù)且δk＝[lxxklxyklxzklyyklyzklzzklxklyklzkmkfvkfckiak]^t；invdyn(·)為機(jī)器人逆動力學(xué)模型函數(shù)；

為了便于進(jìn)行串聯(lián)機(jī)器人動力學(xué)參數(shù)辨識，需要將式(1)改寫成如式(2)所示的線性形式

式中，被稱作回歸矩陣，是關(guān)于關(guān)節(jié)的轉(zhuǎn)角、角速度和角加速度的函數(shù)；

s2.2、依次令矢量δ的第i個動力學(xué)參數(shù)等于1，其余動力學(xué)參數(shù)等于0，然后代入式(1)所示的invdyn(·)函數(shù)中，則除去零項后，剩余項均為第i個動力學(xué)參數(shù)的相關(guān)項，將其賦值給回歸矩陣h的相關(guān)列，最終得到完整的回歸矩陣h。

具體而言，所述步驟s3包含如下步驟：

s3.1、可以將矩陣和機(jī)器人標(biāo)準(zhǔn)動力學(xué)參數(shù)δ分成兩部分，則動力學(xué)方程可以寫成如下的形式

式中，hb為矩陣所有nb個線性無關(guān)列組成的子矩陣；hd為剩下的nd個全零列向量和線性相關(guān)列組成的子矩陣；δb為基本動力學(xué)參數(shù)；δd為對動力學(xué)不起作用的動力學(xué)參數(shù)；

hd可以用hb線性表示，寫作

hd＝hbkd；(4)

s3.2、設(shè)置換矩陣p＝[pbpd]滿足如下式

hp＝[hbhd]，(5)

則有

聯(lián)立式(3)到(6)可以得到

hb(δb+kdδd)＝γ，(7)

寫成含基本動力學(xué)參數(shù)項的動力學(xué)方程，則為

式中，β為基本動力學(xué)參數(shù)，β＝kδ，k＝pb^t+kdpd^t；

s3.3、隨機(jī)生成s組且滿足s遠(yuǎn)遠(yuǎn)大于機(jī)器人標(biāo)準(zhǔn)動力學(xué)參數(shù)δ的個數(shù)；將s組代入回歸矩陣函數(shù)可以得到回歸矩陣hs；對hs使用qr分解可以得到正則上三角矩陣r

遍歷矩陣r的對角元素，記錄其中非零對角元素在矩陣中的列號，依次寫入數(shù)組db；定義行列數(shù)等于機(jī)器人標(biāo)準(zhǔn)動力學(xué)參數(shù)δ個數(shù)的單位矩陣p'；依照數(shù)組db記錄的列號，按順序從單位矩陣p'中取出對應(yīng)的列組成新矩陣，該矩陣為pb，剩余的列組成新矩陣，該矩陣為pd，則p＝[pbpd]。

對矩陣hb,hd分別使用qr分解，可得

則有

聯(lián)立式(5)和式(10)，可以得到

kd＝rb^-1rd，(12)

綜上所述，可以得到pb,kd,pd，進(jìn)而得到基本動力學(xué)參數(shù)β。

上述實施例為本方面較佳的實施方式，但本方明的實施方式并不受上述實施例的限制，其他的任何背離本發(fā)明的精神實質(zhì)與原理下所作的改變、修飾、替代、組合、簡化，均應(yīng)為等效的置換方式，都包含在本發(fā)明的保護(hù)范圍之內(nèi)。