一種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法及系統(tǒng)的制作方法
【專利摘要】一種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法及系統(tǒng)，方法包括：S1、建立機(jī)器人的關(guān)節(jié)力矩與速度以及加速度之間的不顯含重力、僅顯含與位置相關(guān)的慣性力、離心力和科里奧利力的動(dòng)力學(xué)模型：S2、根據(jù)設(shè)定的位置、速度以及加速度，計(jì)算每個(gè)采樣周期的各關(guān)節(jié)的力矩τia(nT)，且根據(jù)各關(guān)節(jié)力矩的力矩約束范圍[τimin,τimax]，計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增益系數(shù)λi(nT)；S3、基于速度增益系數(shù)λi(nT)和電機(jī)最大允許速度計(jì)算機(jī)器人最大速度。本發(fā)明考慮了各關(guān)節(jié)力矩的力矩約束，使得求解的最大速度必為機(jī)器人最大可承受速度；所求取的最大速度，僅與機(jī)器人具體的運(yùn)動(dòng)軌跡有關(guān)，與先前的工作區(qū)域無關(guān)，提高了機(jī)器人的利用效率；進(jìn)一步的，還可求取每個(gè)位置點(diǎn)可承受的最大速度。
【專利說明】
-種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法及系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及機(jī)器人控制領(lǐng)域，尤其設(shè)及一種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲 取方法及系統(tǒng)。
【背景技術(shù)】
[0002] 工業(yè)機(jī)器人在進(jìn)行軌跡規(guī)劃時(shí)，為使機(jī)器人工作效率最大化，需確定機(jī)器人的最 大運(yùn)行速度限制。目前機(jī)器人最大運(yùn)行速度的限制主要有兩個(gè)，即機(jī)器人關(guān)節(jié)最大速度限 制和機(jī)器人末端最大速度限制。關(guān)節(jié)最大速度限制主要W最大關(guān)節(jié)速度為依據(jù)，或根據(jù)電 機(jī)轉(zhuǎn)速力矩曲線確定在滿足輸出轉(zhuǎn)矩條件下的最大電機(jī)速度限制。機(jī)器人末端最大速度限 制主要W機(jī)器人典型工作區(qū)間的最大速度為參考依據(jù)。
[0003] 針對(duì)關(guān)節(jié)最大速度限制，由于機(jī)器人在不同位姿下，其實(shí)際可承受的最大關(guān)節(jié)速 度不同，甚至?xí)霈F(xiàn)關(guān)節(jié)最大速度限制大于機(jī)器人實(shí)際可承受的最大關(guān)節(jié)速度，運(yùn)勢(shì)必引 起機(jī)器人故障停機(jī)，造成機(jī)械本體的損壞。
[0004] 針對(duì)末端最大速度，同樣存在機(jī)器人末端最大速度與機(jī)器人位姿相關(guān)的問題。雖 然可W通過雅克比矩陣建立機(jī)器人關(guān)節(jié)速度和機(jī)器人末端速度的映射關(guān)系，但如果單純W 最大關(guān)節(jié)速度計(jì)算最大末端速度，而不考慮機(jī)器人各關(guān)節(jié)實(shí)際輸出力矩的限制，機(jī)器人可 能因關(guān)節(jié)力矩超限而無法承受該最大速度。因此，在實(shí)際使用過程中，機(jī)器人最大末端速度 主要W典型工作區(qū)域的最大速度為參考。為使機(jī)器人能在較大工作空間中正常運(yùn)行，其典 型工作區(qū)域需盡可能大。但工作區(qū)域越大，對(duì)應(yīng)的可滿足整個(gè)工作區(qū)域正常運(yùn)行的最大速 度限制也就越小，運(yùn)將嚴(yán)重降低機(jī)器人的工作效率。
【發(fā)明內(nèi)容】
[0005] 本發(fā)明要解決的技術(shù)問題在于，針對(duì)現(xiàn)有技術(shù)的上述缺陷，提供一種基于動(dòng)力學(xué) 模型的機(jī)器人最大速度獲取方法及系統(tǒng)。
[0006] 本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是:構(gòu)造一種基于動(dòng)力學(xué)模型的機(jī)器人 最大速度獲取方法，包括W下步驟：
[0007] S1、建立機(jī)器人的關(guān)節(jié)力矩與速度W及加速度之間的不顯含重力、僅顯含與位置 相關(guān)的慣性力、離屯、力和科單奧利力的動(dòng)力學(xué)橫巧：
[000引
CA)
[0009] 其中，Ta(t)表示NXl的關(guān)節(jié)力矩向量，B(q(t))為NXN慣量矩陣，C(q(t)，^-))為N XN科里奧利力及離屯、力矩陣，N為機(jī)器人關(guān)節(jié)數(shù)，;；始)為加速度信息，為速度信息，q(t) 為位置信息；
[0010] S2、根據(jù)設(shè)定的位置、速度W及加速度，計(jì)算每個(gè)采樣周期的各關(guān)節(jié)的力矩Tia (nT)，且根據(jù)各關(guān)節(jié)力矩的力矩約束范圍[Timin，Timax]，計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期 的速度增益系數(shù)、(nT);
[0011] S3、基于步驟S2中的速度增益系數(shù)、(nT)和電機(jī)最大允許速度^。，。,。1.計(jì)算獲得機(jī)器 人最大速度。
[0012] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法中，所述步驟S2中， 通過W下公式計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增益系數(shù)、(nT):
[0013]
B；
[0014] 其中，Timin<0且Timax>0;n表示采樣序號(hào)，n為大于等于0的正整數(shù)，i表示關(guān)節(jié)序 號(hào)，i 二 1,2,...,No
[0015] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法中，步驟S2力矩Tia (nT)的計(jì)算包括:設(shè)定運(yùn)動(dòng)軌跡的起止位置、各關(guān)節(jié)運(yùn)行速度的恒定值、各關(guān)節(jié)加速度值 為0、機(jī)器人采樣周期為T，基于步驟Sl中的所述動(dòng)力學(xué)模型，計(jì)算得到對(duì)應(yīng)于每個(gè)采樣周期 的各關(guān)節(jié)力矩為Tia(nT)。
[0016] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法中，所述步驟S3包 括：
[0017] S31、從所有的關(guān)節(jié)的所有的周期下的速度增益系數(shù)中篩選出最小的速度增益系 數(shù)作為本次運(yùn)動(dòng)軌跡的速度增益系數(shù)：
[001 引
[0019] S32、根據(jù)步驟S31選出的速度增益系數(shù)AW及步驟S2中設(shè)定的運(yùn)行速度^計(jì)算得 到適用于所有位置點(diǎn)的全局最大速度為^ ，并從義^和電機(jī)最大允許速度中選取最 小值作為本次運(yùn)動(dòng)軌跡的最大速運(yùn)
[0020] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法中，所述步驟S3包 括：
[0021] S31'、從每個(gè)采樣周期下的所有的關(guān)節(jié)的速度增益系數(shù)中篩選出最小的速度增益 系數(shù)作為本次運(yùn)動(dòng)軌跡的每個(gè)位置點(diǎn)的速度增益系數(shù)：
[0022] A(nT) =min{A^i(nT) ,A^2(nT),......,A^N(nT)}
[0023] S32'、根據(jù)步驟S31'選出的速度增益系數(shù)MnT) W及步驟S2中設(shè)定的運(yùn)行速度4 計(jì)算得到某個(gè)位置點(diǎn)的局部最大速度為并從地和電機(jī)最大允許速度中選取 最小值作為本次運(yùn)動(dòng)軌跡的某個(gè)位置點(diǎn)的最大速度
[0024] 本發(fā)明還公開了一種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)，包括：
[0025] 模型建立單元，用于建立機(jī)器人的關(guān)節(jié)力矩與速度W及加速度之間的不顯含重 力、僅顯含與位置相關(guān)的慣性力、離屯、力和科里奧利力的動(dòng)力學(xué)模型：
[0026]
(A)
[0027] 其中，Ta(t)表示NXl的關(guān)節(jié)力矩向量，B(q(t))為NX州貫量矩陣，C(q(t)，;肋)為N XN科里奧利力及離屯、力矩陣，N為機(jī)器人關(guān)節(jié)數(shù)，;為加速度信息(f)為速度信息，q(t) 為位置信息；
[0028] 速度增益系數(shù)求解單元，根據(jù)設(shè)定的位置、速度W及加速度，計(jì)算每個(gè)采樣周期的 各關(guān)節(jié)的力矩Tia(nT)，且根據(jù)各關(guān)節(jié)力矩的力矩約束范圍[Timin，TimaxL計(jì)算得到各個(gè)關(guān)節(jié) 的每個(gè)采樣周期的速度增益系數(shù)、(nT);
[0029] 最大速度求解單元，基于速度增益系數(shù)求解單元中得到的速度增益系數(shù)、(nT)和 電機(jī)最大允許速度^ ,計(jì)算機(jī)器人最大速度。 3 W位化廣
[0030] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)中，所述速度增益系 數(shù)求解單元通過W下公式計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增益系數(shù)、(nT):
[0031]
(B)
[0032] 其中，Timin<0且Timax>0 ;n表示采樣序號(hào)，n為大于等于0的正整數(shù)，i表示關(guān)節(jié)序 號(hào)，i = l，2,. . .，N。
[0033] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)中，力矩Tia(nT)的計(jì) 算包括:設(shè)定運(yùn)動(dòng)軌跡的起止位置、各關(guān)節(jié)運(yùn)行速度的恒定值^、各關(guān)節(jié)加速度值為0、機(jī)器 人采樣周期為T，基于所述動(dòng)力學(xué)模型，計(jì)算得到對(duì)應(yīng)于每個(gè)采樣周期的各關(guān)節(jié)力矩為Tia (nT)。
[0034] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)中，最大速度求解單 元包括：
[0035] 速度增益系數(shù)篩選子單元，從所有的關(guān)節(jié)的所有的周期下的速度增益系數(shù)中篩選 出最小的速度增益系數(shù)作為本次運(yùn)動(dòng)軌跡的速度增益系數(shù)：
[0036]
[0037] 全局最大速度求解子單元，根據(jù)速度增益系數(shù)篩選子單元選出的速度增益系數(shù)入 W及速度增益系數(shù)求解單元中設(shè)定的運(yùn)行速度計(jì)算得到適用于所有位置點(diǎn)的全局最大
速度為;I ，并從1 和電機(jī)最大允許速度中選取最小值作為本次運(yùn)動(dòng)軌跡的最大速 ^ ? ^'9'IiiaX
[0038] 在本發(fā)明所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)中，最大速度求解單 元包括：
[0039] 速度增益系數(shù)篩選子單元，從每個(gè)采樣周期下的所有的關(guān)節(jié)的速度增益系數(shù)中篩 選出最小的速度增益系數(shù)作為本次運(yùn)動(dòng)軌跡的每個(gè)位置點(diǎn)的速度增益系數(shù)：
[0040] A(nT) =min{A\(nT) ,A^2(nT),......,入\(。了）}
[0041] 局部最大速度求解子單元，根據(jù)速度增益系數(shù)篩選子單元選出的速度增益系數(shù)入 (nT) W及速度增益系數(shù)求解單元中設(shè)定的運(yùn)行速度計(jì)算得到某個(gè)位置點(diǎn)的局部最大速 度為義(.6巧9。，并從2知巧9。和電機(jī)最大允許速度中選取最小值作為本次運(yùn)動(dòng)軌跡的 某個(gè)位置點(diǎn)的最大速度<^"。、(/^7')
[0042] 實(shí)施本發(fā)明的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法及系統(tǒng)，具有W下有益 效果:本發(fā)明基于動(dòng)力學(xué)模型，考慮了各關(guān)節(jié)力矩的力矩約束，使得求解的最大速度必為機(jī) 器人最大可承受速度;所求取的最大速度，僅與機(jī)器人具體的運(yùn)動(dòng)軌跡有關(guān)，與先前的工作 區(qū)域無關(guān)，極大提高了機(jī)器人的利用效率;進(jìn)一步的，本發(fā)明既可W求取機(jī)器人運(yùn)動(dòng)時(shí)適用 于所有位置的最大速度，又可W求取機(jī)器人運(yùn)動(dòng)時(shí)每個(gè)位置點(diǎn)可承受的最大速度，其屬于 高級(jí)軌跡規(guī)劃控制器變最大速度控制的關(guān)鍵參數(shù)，可最大化機(jī)器人工作效率。
【附圖說明】
[0043] 下面將結(jié)合附圖及實(shí)施例對(duì)本發(fā)明作進(jìn)一步說明，附圖中：
[0044] 圖1是本發(fā)明的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法的流程圖；
[0045] 圖2為初始速度下機(jī)器人各關(guān)節(jié)力矩圖；
[0046] 圖3為初始速度下各個(gè)位置點(diǎn)最大的A允許值示意圖；
[0047] 圖4為最大速度下機(jī)器人各關(guān)節(jié)力矩圖。
【具體實(shí)施方式】
[004引為了對(duì)本發(fā)明的技術(shù)特征、目的和效果有更加清楚的理解，現(xiàn)對(duì)照附圖詳細(xì)說明 本發(fā)明的【具體實(shí)施方式】。
[0049] 參考圖1，是本發(fā)明的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法的流程圖。
[0050] 本發(fā)明的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法包括W下步驟：
[0051] S1、建立機(jī)器人的關(guān)節(jié)力矩與速度W及加速度之間的不顯含重力、僅顯含與位置 相關(guān)的慣性力、離屯、力和科里奧利力的動(dòng)力學(xué)模型：
[0化2]
(A)
[0053] 其中，Ta(t)表示NXl的關(guān)節(jié)力矩向量，B(q(t))為NXN慣量矩陣，c(g(f)，;的）為N XN科里奧利力及離屯、力矩陣，N為機(jī)器人關(guān)節(jié)數(shù)，；；(巧為加速度信息，^狂)為速度信息，q(t) 為位置信息；
[0054] S2、根據(jù)設(shè)定的位置、速度W及加速度，計(jì)算每個(gè)采樣周期的各關(guān)節(jié)的力矩Tia (nT)，且根據(jù)各關(guān)節(jié)力矩的力矩約束范圍[Timin，Timax]，計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期 的速度增益系數(shù)、(nT);
[0化日]S3、基于步驟S2中的速度增益系數(shù)、(nT)和電機(jī)最大允許速度 計(jì)算機(jī)器人最 'ImoUxr 大速度。
[0056]下面詳細(xì)介紹各個(gè)步驟。
[0化7] 關(guān)于步驟Sl:
[005引眾所周知，具有標(biāo)準(zhǔn)形式的機(jī)器人動(dòng)力學(xué)模型的表達(dá)式如下：
[0059]
(1)
[0060] 其中，嗦示NXl的關(guān)節(jié)力矩向量，B(q(t))為NXN慣量矩陣，C(q(t)，;樹）為NXN 科里奧利力及離屯、力矩陣，G(q(t))為NXl的重力向量，N為機(jī)器人關(guān)節(jié)數(shù)，；為加速度信 息，為速度信息，q (t)為位置信息；
[0061] 步驟Sl的動(dòng)力學(xué)模型是對(duì)公式（1)的機(jī)器人標(biāo)準(zhǔn)動(dòng)力學(xué)模型進(jìn)行簡(jiǎn)單的數(shù)學(xué)變 換，使力矩Ta(t)僅與慣性力、科里奧利力、速度、加速度有關(guān)，而與重力無關(guān)，改寫過程為：
[0062]
[0063] 關(guān)于步驟S2:
[0064] 步驟S2中通過W下公式計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增益系數(shù)、 (nT)：
[0065]
CB)
[0066] 其中，Timin<0且Timax>0;n表示采樣序號(hào)，n為大于等于0的正整數(shù)，i表示關(guān)節(jié)序 號(hào)，i二1,2,...,N。
[0067] 其中，公式(B)的推導(dǎo)過程如下：
[0068] 對(duì)公式(A)的動(dòng)力學(xué)模型進(jìn)行數(shù)學(xué)變換，假設(shè)S = U,則：
[0071] 將上面的公式(BI)和公式(B2)代入公式(A)，則可W推導(dǎo)得到公式(C):
[0069]
[0070] 腿）
[0072]
(C)
[0073] 目P，從公式(C)可W得到速度增益系數(shù)A、力矩Ta(t)、力矩Ta(S)之間的關(guān)系如公式 (D)：
[0074]
CD)
[0075] 基于公式化），我們把力矩Tia(nT)作為公式(D)中的Ta(S),各力矩約束范圍作為公 式(D)中的Ta(t)，于是可W計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增益系數(shù)。由于公式 (D)中的A2肯定大于等于0,又TiminCO且Timax>0,所W，在Tia(nT) >〇Tia(nT)時(shí)，則選擇Timax 代入公式(D)，在Tia(nT)<0時(shí)，則選擇Timin代入公式(D)，于是可W得到上述的(B)。
[0076] 從公式(A)可知，各關(guān)節(jié)力矩Ta的計(jì)算依賴于速度、加速度W及位置，所W-個(gè)具 體實(shí)施例中，步驟S2力矩Tia(nT)的計(jì)算具體為:設(shè)定運(yùn)動(dòng)軌跡的起止位置、各關(guān)節(jié)運(yùn)行速度 的恒定值^<，、各關(guān)節(jié)加速度值為〇、機(jī)器人采樣周期為1'，基于步驟51中的所述動(dòng)力學(xué)模型的 公式(A)，計(jì)算得到對(duì)應(yīng)于每個(gè)采樣周期的各關(guān)節(jié)力矩為Tia(nT)。
[0077] 關(guān)于步驟S3:
[0078] 步驟S3求取的最大速度有兩種，一種是適用于所有位置的全局最大速度，一種是 針對(duì)每個(gè)位置的局部最大速度。
[0079] 當(dāng)求取全局最大速度時(shí)，所述步驟S3包括：
[0080] S31、從所有的關(guān)節(jié)的所有的周期下的速度增益系數(shù)中篩選出最小的速度增益系 數(shù)作為本次運(yùn)動(dòng)軌跡的速度增益系數(shù)：
[0081]
[0082] S32、根據(jù)步驟S31選出的速度增益系數(shù)AW及步驟S2中設(shè)定的運(yùn)行速度計(jì)算得 到適用于所有位置點(diǎn)的全局最大速度為^ (7a ,并從^ 和電機(jī)最大允許速度?中選取最 小值作為本次運(yùn)動(dòng)軌跡的最大速度
[0083] 當(dāng)求取局部最大速度時(shí)，所述步驟S3包括：
[0084] S31'、從每個(gè)采樣周期下的所有的關(guān)節(jié)的速度增益系數(shù)中篩選出最小的速度增益 系數(shù)作為本次運(yùn)動(dòng)軌跡的每個(gè)位置點(diǎn)的速度增益系數(shù)：
[00化]A(nT) =min{A(nT),入22(nT),......,入\(。了）}
[00化]S32'、根據(jù)步驟S31'選出的速度增益系數(shù)MnT) W及步驟S2中設(shè)定的運(yùn)行速度 計(jì)算得到某個(gè)位置點(diǎn)的局部最大速度為抓町J。，并從如。和電機(jī)最大允許速度I,。,。沖選取 最小值作為本次運(yùn)動(dòng)軌跡的某個(gè)位置點(diǎn)的最大速度;^、(nr):
[0087]下面結(jié)合具體的實(shí)施案例對(duì)本發(fā)明作進(jìn)一步說明。
[008引現(xiàn)W某SCARA機(jī)器人第一、二關(guān)節(jié)為最大速度求解實(shí)施對(duì)象，首先建立動(dòng)力學(xué)模型 如下：
[0089]
[0090] 其中，/?為第1，2連桿繞質(zhì)屯、坐標(biāo)系巧由的轉(zhuǎn)動(dòng)慣量，聽，：W/;為第1，2連桿質(zhì) 量(含電機(jī)等轉(zhuǎn)配部件），曰1，曰2為第1，2連桿長(zhǎng)度，h，b為第1，2連桿質(zhì)屯、距離連桿起始處的 長(zhǎng)度，qi，q2,違，4，i，;為第1，2連桿旋轉(zhuǎn)的角度，速度及加速度，31，。1為第1個(gè)連桿 位置的正余弦運(yùn)算，Tl，T2為第1，2連桿的力矩。
[0091] 按照上述步驟S2,設(shè)定初始第一二關(guān)節(jié)起止位置為[-120,120]°，設(shè)定第一二關(guān)節(jié) 速度4均為600°/s,設(shè)置加速度為0,可得在該組軌跡參數(shù)下，各關(guān)節(jié)運(yùn)動(dòng)的力矩Tia(nT)及 速度曲線分別如圖2所示。根據(jù)上述步驟S2設(shè)置第一二關(guān)節(jié)力矩限制均為[-63.5侃1， 63.5Nm]，可求得針對(duì)每一個(gè)位置點(diǎn)的可取的M直并繪制成曲線如圖3所示，該圖中最小的被 允許的M直為1.7874,為說明本發(fā)明的有效性，此處暫不考慮電機(jī)最大轉(zhuǎn)速，則最大速度為 1.7874*600Vs = 1072.4°/s，計(jì)算在上述最大速度運(yùn)動(dòng)情況下各關(guān)節(jié)力矩曲線如圖4所示。
[0092] 通過圖4可W看出，當(dāng)機(jī)器人W最大速度1072.4°/s運(yùn)行時(shí)，第一關(guān)節(jié)電機(jī)實(shí)際輸 出轉(zhuǎn)矩在某些位置可達(dá)最大關(guān)節(jié)電機(jī)輸出扭矩，運(yùn)說明此時(shí)機(jī)器人運(yùn)行的速度確實(shí)是機(jī)器 人最大可承受的速度;但在任何一個(gè)位置點(diǎn)，各關(guān)節(jié)實(shí)際輸出扭矩均小于或等于最大關(guān)節(jié) 扭矩限制值，運(yùn)說明了機(jī)器人此速度下運(yùn)行是切實(shí)可行的。W上兩個(gè)方面證明了基于本發(fā) 明所求解的最大速度在保證機(jī)器人正常運(yùn)行的前提下，切實(shí)提高了機(jī)器人的利用效率。
[0093] 相應(yīng)的，本發(fā)明還公開了一種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)，包括：
[0094] 模型建立單元，用于建立機(jī)器人的關(guān)節(jié)力矩與速度W及加速度之間的不顯含重 力、僅顯含與位置相關(guān)的慣性力、離屯、力和科里奧利力的動(dòng)力學(xué)模型：
[0095]
? (A)
[0096] 其中，Ta(t)表示N X 1的關(guān)節(jié)力矩向量，B(q(t))為N X N慣量矩陣，C(如)，* (f))為N XN科里奧利力及離屯、力矩陣，N為機(jī)器人關(guān)節(jié)數(shù)，為加速度信息，^幻為速度信息，q(t) 為位置信息；
[0097] 速度增益系數(shù)求解單元，根據(jù)設(shè)定的位置、速度W及加速度，計(jì)算每個(gè)采樣周期的 各關(guān)節(jié)的力矩Tia(nT)，且根據(jù)各關(guān)節(jié)力矩的力矩約束范圍[Timin，TimaxL計(jì)算得到各個(gè)關(guān)節(jié) 的每個(gè)采樣周期的速度增益系數(shù)、(nT);
[0098] 最大速度求解單元，基于速度增益系數(shù)求解單元中得到的速度增益系數(shù)、(nT)和 電機(jī)最大允許速度^ ,計(jì)算機(jī)器人最大速度。 i mutof
[0099] 其中，所述速度增益系數(shù)求解單元通過W下公式計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周 期的速度增益系數(shù)、(nT):
[0100]
化）
[0101] 其中，Timin<0且Timax>0 ;n表示采樣序號(hào)，n為大于等于0的正整數(shù)，i表示關(guān)節(jié)序 號(hào)，i = l，2,. . .，N。
[0102] 其中，力矩Tia(nT)的計(jì)算包括:設(shè)定運(yùn)動(dòng)軌跡的起止位置、各關(guān)節(jié)運(yùn)行速度的恒定 值i .、各關(guān)節(jié)加速度值為0、機(jī)器人采樣周期為T，基于所述動(dòng)力學(xué)模型，計(jì)算得到對(duì)應(yīng)于每 個(gè)采樣周期的各關(guān)節(jié)力矩為Tia(nT)。
[0103] 其中，最大速度求解單元包括：
[0104] 速度增益系數(shù)篩選子單元，從所有的關(guān)節(jié)的所有的周期下的速度增益系數(shù)中篩選 出最小的速度增益系數(shù)作為本次運(yùn)動(dòng)軌跡的速度增益系數(shù)：
[0105]
[0106] 全局最大速度求解子單元，根據(jù)速度增益系數(shù)篩選子單元選出的速度增益系數(shù)入 W及速度增益系數(shù)求解單元中設(shè)定的運(yùn)行速度計(jì)算得到適用于所有位置點(diǎn)的全局最大 速度為義4，并從/1J。和電機(jī)最大允許速度;中選取最小值作為本次運(yùn)動(dòng)軌跡的最大速 謹(jǐn)；：
[0107] 或者，最大速度求解單元包括：
[0108] 速度增益系數(shù)篩選子單元，從每個(gè)采樣周期下的所有的關(guān)節(jié)的速度增益系數(shù)中篩 選出最小的速度增益系數(shù)作為本次運(yùn)動(dòng)軌跡的每個(gè)位置點(diǎn)的速度增益系數(shù)：
[0109] A(nT) =min{A^i(nT) ,A^2(nT),......,A^N(nT)}
[0110] 局部最大速度求解子單元，根據(jù)速度增益系數(shù)篩選子單元選出的速度增益系數(shù)入 (nT) W及速度增益系數(shù)求解單元中設(shè)定的運(yùn)行速度計(jì)算得到某個(gè)位置點(diǎn)的局部最大速 度為/1(?乃，并從ibr) g'。和電機(jī)最大允許速度中選取最小值作為本次運(yùn)動(dòng)軌跡的 某個(gè)位置點(diǎn)的最大速度
[0111] 綜上所述，實(shí)施本發(fā)明的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法及系統(tǒng)，具 有W下有益效果:本發(fā)明基于動(dòng)力學(xué)模型，考慮了各關(guān)節(jié)力矩的力矩約束，使得求解的最大 速度必為機(jī)器人最大可承受速度;所求取的最大速度，僅與機(jī)器人具體的運(yùn)動(dòng)軌跡有關(guān)，與 先前的工作區(qū)域無關(guān)，極大提高了機(jī)器人的利用效率;進(jìn)一步的，本發(fā)明既可W求取機(jī)器人 運(yùn)動(dòng)時(shí)適用于所有位置的最大速度，又可W求取機(jī)器人運(yùn)動(dòng)時(shí)每個(gè)位置點(diǎn)可承受的最大速 度，其屬于高級(jí)軌跡規(guī)劃控制器變最大速度控制的關(guān)鍵參數(shù)，可最大化機(jī)器人工作效率。
[0112]上面結(jié)合附圖對(duì)本發(fā)明的實(shí)施例進(jìn)行了描述，但是本發(fā)明并不局限于上述的具體 實(shí)施方式，上述的【具體實(shí)施方式】?jī)H僅是示意性的，而不是限制性的，本領(lǐng)域的普通技術(shù)人員 在本發(fā)明的啟示下，在不脫離本發(fā)明宗旨和權(quán)利要求所保護(hù)的范圍情況下，還可做出很多 形式，運(yùn)些均屬于本發(fā)明的保護(hù)之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法，其特征在于，包括以下步驟： 51、 建立機(jī)器人的關(guān)節(jié)力矩與速度以及加速度之間的不顯含重力、僅顯含與位置相關(guān) 的慣性力、離心力和科里奧利力的動(dòng)力學(xué)模型：其中，Ta(t)表示NXl的關(guān)節(jié)力矩向量，B(q(t))為NXN慣量矩陣，C(Wr)，)為NXN科 里奧利力及離心力矩陣，N為機(jī)器人關(guān)節(jié)數(shù)，為加速度信息為速度信息，q(t)為位 置信息； 52、 根據(jù)設(shè)定的位置、速度以及加速度，計(jì)算每個(gè)采樣周期的各關(guān)節(jié)的力矩ila(nT)，且 根據(jù)各關(guān)節(jié)力矩的力矩約束范圍[τ_，T imax]，計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增 益系數(shù)M(nT); 53、 基于步驟S2中的速度增益系數(shù)A1(I1T)和電機(jī)最大允許速度計(jì)算獲得機(jī)器人最 大速度。2. 根據(jù)權(quán)利要求1所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法，其特征在于，所 述步驟S2中，通過以下公式計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增益系數(shù)M(nT):其中，11"1"<0且11_>0 ;n表示采樣序號(hào)，η為大于等于O的正整數(shù)，i表示關(guān)節(jié)序號(hào)，i = 1，2,···，N〇3. 根據(jù)權(quán)利要求1所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法，其特征在于，步 驟S2力矩Tla(nT)的計(jì)算包括:設(shè)定運(yùn)動(dòng)軌跡的起止位置、各關(guān)節(jié)運(yùn)行速度的恒定值各 關(guān)節(jié)加速度值為〇、機(jī)器人采樣周期為T，基于步驟Sl中的所述動(dòng)力學(xué)模型，計(jì)算得到對(duì)應(yīng)于 每個(gè)采樣周期的各關(guān)節(jié)力矩為ila(nT)。4. 根據(jù)權(quán)利要求3所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法，其特征在于，所 述步驟S3包括： 531、 從所有的關(guān)節(jié)的所有的周期下的速度增益系數(shù)中篩選出最小的速度增益系數(shù)作 為本次運(yùn)動(dòng)軌跡的諫度增益系數(shù)：532、 根據(jù)步驟S31選出的速度增益系數(shù)λ以及步驟S2中設(shè)定的運(yùn)行速度^計(jì)算得到適用 于所有位置點(diǎn)的全局最大速度為;I ^，并從Λ ^和電機(jī)最大允許速度中選取最小值作 為本次運(yùn)動(dòng)軌跡的最大速度a,lu = HiinU(^gwwii)05. 根據(jù)權(quán)利要求3所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取方法，其特征在于，所 述步驟S3包括： S31'、從每個(gè)采樣周期下的所有的關(guān)節(jié)的速度增益系數(shù)中篩選出最小的速度增益系數(shù) 作為太汝沄動(dòng)軌協(xié)的毎個(gè)份詈?的諫庶增益系教，S32'、根據(jù)步驟S31'選出的速度增益系數(shù)λ(ηΤ)以及步驟S2中設(shè)定的運(yùn)行速度^計(jì)算得 到某個(gè)位置點(diǎn)的局部最大速度為Κ/7Π ^，并從辦^和電機(jī)最大允許速度中選取最小 值作為本次運(yùn)動(dòng)軌跡的某個(gè)位置點(diǎn)的最大速度<^>門^>7) = ininUQn 。6. -種基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)，其特征在于，包括： 模型建立單元，用于建立機(jī)器人的關(guān)節(jié)力矩與速度以及加速度之間的不顯含重力、僅 顯含與位置相關(guān)的慣性力、離心力和科里奧利力的動(dòng)力學(xué)模型：其中，Ta⑴表示NXl的關(guān)節(jié)力矩向量，B(q(t))為NXN慣量矩陣，"％)，^))為NXN科 里奧利力及離心力矩陣，N為機(jī)器人關(guān)節(jié)數(shù)，為加速度信息，為速度信息，q(t)為位 置信息； 速度增益系數(shù)求解單元，根據(jù)設(shè)定的位置、速度以及加速度，計(jì)算每個(gè)采樣周期的各關(guān) 節(jié)的力矩ila(nT)，且根據(jù)各關(guān)節(jié)力矩的力矩約束范圍[Timin，Timax]，計(jì)算得到各個(gè)關(guān)節(jié)的每 個(gè)采樣周期的速度增益系數(shù)M(nT); 最大速度求解單元，基于速度增益系數(shù)求解單元中得到的速度增益系數(shù)M(nT)和電機(jī) 最大允許速度^ 計(jì)算機(jī)器人最大速度。 ? motor7. 根據(jù)權(quán)利要求6所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)，其特征在于，所 述速度增益系數(shù)求解單元通過以下公式計(jì)算得到各個(gè)關(guān)節(jié)的每個(gè)采樣周期的速度增益系 數(shù) Μ(ηΤ):其中，11"1"<0且11_>0 ;η表示采樣序號(hào)，η為大于等于O的正整數(shù)，i表示關(guān)節(jié)序號(hào)，i = 1，2,···，N〇8. 根據(jù)權(quán)利要求6所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)，其特征在于，力 矩ila(nT)的計(jì)算包括:設(shè)定運(yùn)動(dòng)軌跡的起止位置、各關(guān)節(jié)運(yùn)行速度的恒定值4、各關(guān)節(jié)加 速度值為〇、機(jī)器人采樣周期為T，基于所述動(dòng)力學(xué)模型，計(jì)算得到對(duì)應(yīng)于每個(gè)采樣周期的各 關(guān)節(jié)力矩為Tia(nT)。9. 根據(jù)權(quán)利要求8所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)，其特征在于，最 大速度求解單元包括： 速度增益系數(shù)篩選子單元，從所有的關(guān)節(jié)的所有的周期下的速度增益系數(shù)中篩選出最 小的速度增益系數(shù)作為本次運(yùn)動(dòng)軌跡的速度增益系數(shù)： A=HiinI^21(O) ,A21(T)，......,λ2ι(ηΤ),全局最大速度求解子單元，根據(jù)速度增益系數(shù)篩選子單元選出的速度增益系數(shù)λ以及 速度增益系數(shù)求解單元中設(shè)定的運(yùn)行速度4計(jì)算得到適用于所有位置點(diǎn)的全局最大速度 為2 ^，并從2 ^和電機(jī)最大允許速度.中選取最小值作為本次運(yùn)動(dòng)軌跡的最大速度10.根據(jù)權(quán)利要求8所述的基于動(dòng)力學(xué)模型的機(jī)器人最大速度獲取系統(tǒng)，其特征在于， 最大速度求解單元包括： 速度增益系數(shù)篩選子單元，從每個(gè)采樣周期下的所有的關(guān)節(jié)的速度增益系數(shù)中篩選出 最小的速度增益系數(shù)作為本次運(yùn)動(dòng)軌跡的每個(gè)位置點(diǎn)的速度增益系數(shù)：局部最大速度求解子單元，根據(jù)速度增益系數(shù)篩選子單元選出的速度增益系數(shù)λ(ηΤ) 以及速度增益系數(shù)求解單元中設(shè)定的運(yùn)行速度^計(jì)算得到某個(gè)位置點(diǎn)的局部最大速度為 Γ) % ,并從Kaf) %和電機(jī)最大允許速度.中選取最小值作為本次運(yùn)動(dòng)軌跡的某個(gè) 位置點(diǎn)的最大速
【文檔編號(hào)】G05B13/04GK106019942SQ201610460754
【公開日】2016年10月12日
【申請(qǐng)日】2016年6月23日
【發(fā)明人】楊磊, 屈云飛
【申請(qǐng)人】深圳市匯川技術(shù)股份有限公司