本發(fā)明涉及機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡的優(yōu)化方法，特別涉及一種快速收斂的有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化方法。

背景技術(shù)：

隨著勞動(dòng)力成本的逐年上升，各行各業(yè)對(duì)于機(jī)器人替代人進(jìn)行工作的需求也逐年上升。許多應(yīng)用場(chǎng)景對(duì)機(jī)器人的運(yùn)動(dòng)性能有著較高的要求，這就需要對(duì)機(jī)器人進(jìn)行基于動(dòng)力學(xué)模型的運(yùn)動(dòng)控制。精確的機(jī)器人動(dòng)力學(xué)參數(shù)是基于動(dòng)力學(xué)模型運(yùn)動(dòng)控制的前提。

一般情況下，機(jī)器人的動(dòng)力學(xué)參數(shù)不能直接獲得，需要通過(guò)實(shí)施辨識(shí)實(shí)驗(yàn)獲取。辨識(shí)實(shí)驗(yàn)的主要過(guò)程為，使用優(yōu)化方法獲得一條最優(yōu)激勵(lì)軌跡，機(jī)器人沿著激勵(lì)軌跡進(jìn)行運(yùn)動(dòng)，采樣機(jī)器人各關(guān)節(jié)的角度和力矩，采樣數(shù)據(jù)經(jīng)過(guò)一定處理之后，代入機(jī)器人逆動(dòng)力學(xué)模型，得到一個(gè)超定的線性方程組，解這個(gè)線性方程組即可得到機(jī)器人動(dòng)力學(xué)模型。

激勵(lì)軌跡通常使用有限傅里葉級(jí)數(shù)激勵(lì)軌跡，現(xiàn)有的有限傅里葉級(jí)數(shù)激勵(lì)軌跡優(yōu)化方法主要的問題，目標(biāo)函數(shù)計(jì)算量大，約束條件過(guò)多，優(yōu)化搜索空間過(guò)大，優(yōu)化容易失敗，優(yōu)化時(shí)間過(guò)長(zhǎng)等。

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

本發(fā)明的目的是提供一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法，旨在解決現(xiàn)有的有限傅里葉級(jí)數(shù)激勵(lì)軌跡優(yōu)化方法目標(biāo)函數(shù)計(jì)算量大，約束條件過(guò)多，優(yōu)化搜索空間過(guò)大，優(yōu)化容易失敗，優(yōu)化時(shí)間過(guò)長(zhǎng)等問題。

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

一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法，包括如下步驟：

S1、定義有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的表達(dá)式：

式中，目標(biāo)函數(shù)為矩陣H的條件數(shù)；優(yōu)化變量為有限傅里葉級(jí)數(shù)系數(shù)約束條件為機(jī)器人各關(guān)節(jié)角度、角速度和角加速度范圍約束；

S2、簡(jiǎn)化有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的目標(biāo)函數(shù)計(jì)算量；

S3、在不改變搜索空間大小的前提下，簡(jiǎn)化有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的約束條件數(shù)目；

S4、計(jì)算有限傅里葉級(jí)數(shù)系數(shù)的取值范圍減小非線性優(yōu)化的搜索空間，加快搜索速度；

S5、使用內(nèi)部點(diǎn)算法進(jìn)行有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化，加快非線性優(yōu)化的收斂速度。

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

S2.1、使用算法時(shí)間復(fù)雜度為線性階O(n)的遞歸牛頓歐拉方法進(jìn)行機(jī)器人逆動(dòng)力學(xué)建模，可以得到如下的機(jī)器人逆動(dòng)力學(xué)模型：

式中，Γ為各關(guān)節(jié)力矩；q,分別為各關(guān)節(jié)的轉(zhuǎn)角、角速度和角加速度；δ為各連桿的標(biāo)準(zhǔn)動(dòng)力學(xué)參數(shù)，且δ＝[δ ₁δ₂ … δ_n]^T，δ_k為連桿k的標(biāo)準(zhǔn)動(dòng)力學(xué)參數(shù)且δ_k＝[L_xxk L_xykL_xzk L_yyk L_yzk L_zzk l_xk l_yk l_zk m_k f_vk f_ck I_ak]^T；

S2.2、對(duì)機(jī)器人動(dòng)力學(xué)方程進(jìn)行回歸矩陣解耦；

S2.3、使用數(shù)值方法將回歸矩陣簡(jiǎn)化為基礎(chǔ)回歸矩陣

S2.4、使用中間變量替代重復(fù)出現(xiàn)項(xiàng)，減少基礎(chǔ)回歸矩陣的計(jì)算量；

S2.5、在一個(gè)有限傅里葉級(jí)數(shù)周期T內(nèi)，以一定的間隔Δt生成時(shí)間序列t，滿足：

t＝[t₁ t₂ … t_T/Δt]^T， (3)

將時(shí)間序列t代入式(4)所示的有限傅里葉級(jí)數(shù)公式，得到T/Δt組

式中，q_i，和分別為關(guān)節(jié)i的角度、角速度和角加速度；ω_f是傅里葉級(jí)數(shù)的基礎(chǔ)角頻率；和是傅里葉級(jí)數(shù)的系數(shù)，其中l(wèi)＝1,2,…,N_i；q_io是關(guān)節(jié)i的偏置；i＝1,2,…,n，n為機(jī)器人的關(guān)節(jié)數(shù)目；

S2.6、將T/Δt組代入基礎(chǔ)回歸矩陣合并成一個(gè)矩陣H_T/Δt，計(jì)算矩陣H_T/Δt的條件數(shù)作為有限傅里葉級(jí)數(shù)激勵(lì)軌跡非線性優(yōu)化的目標(biāo)函數(shù)。

進(jìn)一步地，所述步驟S2.2具體包括：

S2.2.1、依次令矢量δ的第i個(gè)動(dòng)力學(xué)參數(shù)等于1，其余動(dòng)力學(xué)參數(shù)等于0，然后代入所示的InvDYN(·)函數(shù)中，則除去零項(xiàng)后，剩余項(xiàng)均為第i個(gè)動(dòng)力學(xué)參數(shù)的相關(guān)項(xiàng)；

S2.2.2、將剩余項(xiàng)賦值給回歸矩陣H的相關(guān)列，最終得到完整的回歸矩陣H，從而得到解耦之后的機(jī)器人逆動(dòng)力學(xué)模型：

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

進(jìn)一步地，所述的步驟S2.3具體包括步驟：

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

式中，H_b為矩陣所有n_b個(gè)線性無(wú)關(guān)列組成的子矩陣；H_d為剩下的n_d個(gè)全零列向量和線性相關(guān)列組成的子矩陣；δ_b為基礎(chǔ)動(dòng)力學(xué)參數(shù)；δ_d為對(duì)動(dòng)力學(xué)不起作用的動(dòng)力學(xué)參數(shù)；

S2.3.2、設(shè)置換矩陣P＝[P_b P_d]滿足如下式(7)：

HP＝[H_b H_d]， (7)

則有：

H_b＝HP_b， (8)

隨機(jī)生成M組且滿足M>>len(δ)，將M組代入回歸矩陣函數(shù)可以得到回歸矩陣H_M，如式(9)所示，對(duì)H_M使用QR分解可以得到正則上三角矩陣R，

S2.3.3、遍歷矩陣R的對(duì)角元素，記錄其中非零對(duì)角元素在矩陣中的列號(hào)，依次寫入數(shù)組db，定義行列數(shù)等于機(jī)器人標(biāo)準(zhǔn)動(dòng)力學(xué)參數(shù)δ個(gè)數(shù)的單位矩陣P'，依照數(shù)組db記錄的列號(hào)，按順序從單位矩陣P'中取出對(duì)應(yīng)的列組成新矩陣，該矩陣為P_b，將P_b代入式(8)則可以算得基礎(chǔ)回歸矩陣

進(jìn)一步地，所述步驟S2.4具體包括：遍歷基礎(chǔ)回歸矩陣的符號(hào)表達(dá)式，提取重復(fù)出現(xiàn)的項(xiàng)，使用中間變量替換，計(jì)算的時(shí)候，先計(jì)算中間變量，再計(jì)算各項(xiàng)基礎(chǔ)回歸矩陣的元素。

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

S3.1將所述步驟S2.5得到的T/Δt組代入機(jī)器人各關(guān)節(jié)角度、角速度和角加速度的約束條件中：

可以得到6×T/Δt×n組不等式約束，式中，q_min,q_max,分別是各關(guān)節(jié)角度、角速度、角加速度的最小值和最大值，均為n×1的矢量，n為機(jī)器人的關(guān)節(jié)數(shù)目；

S3.2將所述步驟S3.16×T/Δt×n組不等式約束進(jìn)行合并簡(jiǎn)化成6×n不等式約束：

式中，min(·)為求最小值函數(shù)；max(·)為求最大值函數(shù)。

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

S4.1在式(4)的約束范圍內(nèi)，隨機(jī)生成(2×N+1)組代入式(4)所示的有限傅里葉級(jí)數(shù)公式，得到一個(gè)關(guān)于[(2×N+1)×n]個(gè)有限傅里葉級(jí)數(shù)系數(shù)的線性方程組，可以解得一組n個(gè)關(guān)節(jié)的有限傅里葉級(jí)數(shù)系數(shù)

S4.2重復(fù)所述步驟S4.1K次，K盡可能地大，則可以得到K組有限傅里葉級(jí)數(shù)系數(shù)那么有限傅里葉級(jí)數(shù)系數(shù)的約束范圍為：

將式(12)添加入非線性優(yōu)化的約束條件中。

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

S5.1重新定義有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的表達(dá)式：

S5.2使用內(nèi)部點(diǎn)算法計(jì)算式(13)所示的非線性優(yōu)化問題。

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

本發(fā)明使用一系列方法加快有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的收斂速度：包括使用遞歸牛頓歐拉方法進(jìn)行機(jī)器人逆動(dòng)力學(xué)建模，計(jì)算基礎(chǔ)動(dòng)力學(xué)參數(shù)回歸矩陣和使用基于中間變量的回歸矩陣計(jì)算方法簡(jiǎn)化非線性優(yōu)化的目標(biāo)函數(shù)計(jì)算量；合并關(guān)節(jié)角度、角速度、角加速度約束條件以減少約束條件的數(shù)目；計(jì)算傅里葉級(jí)數(shù)系數(shù)的取值范圍減小非線性優(yōu)化的搜索空間，加快搜索速度；使用內(nèi)部點(diǎn)算法加快非線性優(yōu)化的收斂速度。本發(fā)明解決有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化計(jì)算量大，收斂時(shí)間長(zhǎng)的問題。

附圖說(shuō)明

圖1是本發(fā)明實(shí)施例的一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法流程示意圖。

圖2是本發(fā)明實(shí)施例的一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法步驟S2的流程示意圖。

圖3是本發(fā)明實(shí)施例的一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法步驟S3的流程示意圖。

圖4是本發(fā)明實(shí)施例的一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法步驟S4的流程示意圖。

圖5是本發(fā)明實(shí)施例的一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法步驟S5的流程示意圖。

具體實(shí)施方式

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

現(xiàn)以某六自由度垂直關(guān)節(jié)串聯(lián)機(jī)器人為動(dòng)力學(xué)參數(shù)辨識(shí)對(duì)象，按照以下步驟進(jìn)行機(jī)器人辨識(shí)實(shí)驗(yàn)所需要的有限傅里葉級(jí)數(shù)激勵(lì)軌跡非線性優(yōu)化：

一種快速收斂的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)軌跡優(yōu)化方法，包括如下步驟：

S1、定義有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的表達(dá)式：

式中，目標(biāo)函數(shù)為矩陣H的條件數(shù)；優(yōu)化變量為為式(4)所示有限傅里葉級(jí)數(shù)系數(shù)約束條件為機(jī)器人各關(guān)節(jié)角度、角速度和角加速度范圍約束；

S2、簡(jiǎn)化有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的目標(biāo)函數(shù)計(jì)算量；

S3、在不改變搜索空間大小的前提下，簡(jiǎn)化有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的約束條件數(shù)目；

S4、計(jì)算有限傅里葉級(jí)數(shù)系數(shù)的取值范圍減小非線性優(yōu)化的搜索空間，加快搜索速度；

S5、使用內(nèi)部點(diǎn)算法進(jìn)行有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化，加快非線性優(yōu)化的收斂速度。

具體而言，如圖2所示，所述步驟S2包括如下步驟：

S2.1、使用算法時(shí)間復(fù)雜度為線性階O(n)的遞歸牛頓歐拉方法進(jìn)行機(jī)器人逆動(dòng)力學(xué)建模，可以得到如下的機(jī)器人逆動(dòng)力學(xué)模型：

式中，Γ為各關(guān)節(jié)力矩；q,分別為各關(guān)節(jié)的轉(zhuǎn)角、角速度和角加速度；δ為各連桿的標(biāo)準(zhǔn)動(dòng)力學(xué)參數(shù)，且δ＝[δ₁δ₂…δ₆]^T，δ_k為連桿k的標(biāo)準(zhǔn)動(dòng)力學(xué)參數(shù)且δ_k＝[L_xxk L_xyk L_xzkL_yyk L_yzk L_zzk l_xk l_yk l_zk m_k f_vk f_ck I_ak]^T；

S2.2、對(duì)機(jī)器人動(dòng)力學(xué)方程進(jìn)行回歸矩陣解耦；

S2.3、使用數(shù)值方法將回歸矩陣簡(jiǎn)化為基礎(chǔ)回歸矩陣

S2.4、使用中間變量替代重復(fù)出現(xiàn)項(xiàng)，減少基礎(chǔ)回歸矩陣的計(jì)算量；

S2.5、在一個(gè)有限傅里葉級(jí)數(shù)周期12.5秒內(nèi)，以一定的間隔Δt＝0.001秒生成時(shí)間序列t,滿足：

t＝[t₁ t₂ … t₁₂₅₀₀]^T，(3)

將時(shí)間序列t代入式(4)所示的有限傅里葉級(jí)數(shù)公式，得到12500組

式中，N_i＝6，q_i，和分別為關(guān)節(jié)i的角度、角速度和角加速度；ω_f是傅里葉級(jí)數(shù)的基礎(chǔ)角頻率，取ω_f＝0.16π；和是傅里葉級(jí)數(shù)的系數(shù)，其中l(wèi)＝1,2,…,6；q_io是關(guān)節(jié)i的偏置；i＝1,2,…,6；

S2.6、將12500組代入基礎(chǔ)回歸矩陣合并成一個(gè)矩陣H₁₂₅₀₀，計(jì)算矩陣H₁₂₅₀₀的條件數(shù)作為有限傅里葉級(jí)數(shù)激勵(lì)軌跡非線性優(yōu)化的目標(biāo)函數(shù)。

具體而言，所述步驟S2.2具體包括：

S2.2.1、依次令矢量δ的第i個(gè)動(dòng)力學(xué)參數(shù)等于1，其余動(dòng)力學(xué)參數(shù)等于0，然后代入式(2)所示的InvDYN(·)函數(shù)中，則除去零項(xiàng)后，剩余項(xiàng)均為第i個(gè)動(dòng)力學(xué)參數(shù)的相關(guān)項(xiàng)；

S2.2.2、將剩余項(xiàng)賦值給回歸矩陣H的相關(guān)列，最終得到完整的回歸矩陣H，從而得到解耦之后的機(jī)器人逆動(dòng)力學(xué)模型：

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

具體而言，所述的步驟S2.3具體包括步驟：

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

式中，H_b為矩陣所有n_b個(gè)線性無(wú)關(guān)列組成的子矩陣；H_d為剩下的n_d個(gè)全零列向量和線性相關(guān)列組成的子矩陣；δ_b為基礎(chǔ)動(dòng)力學(xué)參數(shù)；δ_d為對(duì)動(dòng)力學(xué)不起作用的動(dòng)力學(xué)參數(shù)；

S2.3.2、設(shè)置換矩陣P＝[P_b P_d]滿足如下式(7)：

HP＝[H_b H_d]， (7)

則有：

H_b＝HP_b， (8)

隨機(jī)生成10000組且滿足M>>len(δ)，將M組代入回歸矩陣函數(shù)可以得到回歸矩陣H_M，如式(9)所示，對(duì)H_M使用QR分解可以得到正則上三角矩陣R，

S2.3.3、遍歷矩陣R的對(duì)角元素，記錄其中非零對(duì)角元素在矩陣中的列號(hào)，依次寫入數(shù)組db，定義行列數(shù)等于機(jī)器人標(biāo)準(zhǔn)動(dòng)力學(xué)參數(shù)δ個(gè)數(shù)的單位矩陣P'，依照數(shù)組db記錄的列號(hào)，按順序從單位矩陣P'中取出對(duì)應(yīng)的列組成新矩陣，該矩陣為P_b，將P_b代入式(8)則可以算得基礎(chǔ)回歸矩陣

具體而言，所述步驟S2.4具體包括：遍歷基礎(chǔ)回歸矩陣的符號(hào)表達(dá)式，提取重復(fù)出現(xiàn)的項(xiàng)，使用中間變量替換，計(jì)算的時(shí)候，先計(jì)算中間變量，再計(jì)算各項(xiàng)基礎(chǔ)回歸矩陣的元素，可以大大減少計(jì)算量。

具體而言，如圖3所示，所述步驟S3包括如下步驟：

S3.1、將所述步驟S2.5得到的12500組代入式(10)所示的機(jī)器人各關(guān)節(jié)角度、角速度和角加速度的約束條件中：

可以得到6×12500×12500組不等式約束，式中，q_min,q_max,分別是各關(guān)節(jié)角度、角速度、角加速度的最小值和最大值，均為6×1的矢量；

S3.2、將所述步驟S3.16×12500×6組不等式約束進(jìn)行合并簡(jiǎn)化成6×6組不等式約束：

式中，min(·)為求最小值函數(shù)；max(·)為求最大值函數(shù)。

具體而言，如圖4所示，所述步驟S4包括如下步驟：

S4.1、在式(4)的約束范圍內(nèi)，隨機(jī)生成(2×6+1)組代入式(9)所示的有限傅里葉級(jí)數(shù)公式，得到一個(gè)關(guān)于[(2×6+1)×6]個(gè)有限傅里葉級(jí)數(shù)系數(shù)的線性方程組，可以解得一組n個(gè)關(guān)節(jié)的有限傅里葉級(jí)數(shù)系數(shù)

S4.2、重復(fù)步驟S4.110000次，則可以得到10000組有限傅里葉級(jí)數(shù)系數(shù)那么有限傅里葉級(jí)數(shù)系數(shù)的約束范圍為：

將式(12)添加入非線性優(yōu)化的約束條件中。

具體而言，如圖5所示，所述步驟S5包括如下步驟：

S5.1、重新定義有限傅里葉級(jí)數(shù)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)激勵(lì)軌跡非線性優(yōu)化的表達(dá)式：

S5.2、使用內(nèi)部點(diǎn)算法計(jì)算式(13)所示的非線性優(yōu)化問題。

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