本發(fā)明屬于工業(yè)機(jī)器人領(lǐng)域，更具體地，涉及一種關(guān)節(jié)機(jī)器人軌跡規(guī)劃方法。
背景技術(shù)：
：機(jī)器人動(dòng)力學(xué)是研究機(jī)器人運(yùn)動(dòng)與受力之間的關(guān)系，建立機(jī)器人動(dòng)力學(xué)模型是機(jī)器人動(dòng)態(tài)控制和高精度運(yùn)動(dòng)控制、動(dòng)態(tài)設(shè)計(jì)及運(yùn)動(dòng)仿真的基礎(chǔ)。目前，機(jī)器人動(dòng)力學(xué)計(jì)算的方法主要有：牛頓-歐拉法，拉格朗日法，凱恩法，旋量定理法。應(yīng)用不同的動(dòng)力學(xué)方法所得到的動(dòng)力學(xué)方程是不同的，其計(jì)算效率有很大差別，牛頓-歐拉方程是采用形位坐標(biāo)作為獨(dú)立變量的傳統(tǒng)力學(xué)方法，在處理少數(shù)關(guān)節(jié)時(shí)，形式簡(jiǎn)潔，在本發(fā)明中選擇牛頓-歐拉法計(jì)算機(jī)器人動(dòng)力學(xué)。機(jī)器人動(dòng)力學(xué)模型的建立依賴于機(jī)器人連桿的運(yùn)動(dòng)參數(shù)和機(jī)器人連桿慣性參數(shù)的確定，前者容易獲得，后者必須加以辨識(shí)。機(jī)器人連桿的慣性參數(shù)辨識(shí)是機(jī)器人動(dòng)力學(xué)的重要部分。現(xiàn)有的機(jī)器人慣性參數(shù)辨識(shí)方法有：解體測(cè)量計(jì)算法，解體實(shí)驗(yàn)測(cè)量法，不解體實(shí)驗(yàn)測(cè)量法，三維模型測(cè)量法，理論辨識(shí)法。理論辨識(shí)法不需要解體機(jī)器人,也不需要專門的實(shí)驗(yàn)平臺(tái)，可以獲得較高的精度，因此，本設(shè)計(jì)采用理論辨識(shí)法辨識(shí)機(jī)器人的慣性參數(shù)。粒子群算法和遺傳算法類似，也是一種基于群體疊代的算法，但并沒有遺傳算法的交叉以及變異，而是粒子在解空間追隨最優(yōu)的粒子進(jìn)行搜索。粒子群算法的優(yōu)勢(shì)在于簡(jiǎn)單，容易實(shí)現(xiàn)，無需梯度信息，參數(shù)少，特別適合于處理實(shí)優(yōu)化問題?，F(xiàn)階段對(duì)機(jī)器人的軌跡規(guī)劃多為基于時(shí)間最優(yōu)和整體能量最優(yōu)，沒有將機(jī)器人關(guān)節(jié)電機(jī)力矩和能量均衡作為機(jī)器人運(yùn)動(dòng)中軌跡的判定條件，難以滿足實(shí)際生產(chǎn)中對(duì)機(jī)器人精確控制的要求。對(duì)于重載機(jī)器人運(yùn)動(dòng)過程抖動(dòng)，能耗過大等問題無法通過運(yùn)動(dòng)學(xué)軌跡規(guī)劃解決。技術(shù)實(shí)現(xiàn)要素：針對(duì)現(xiàn)有技術(shù)的以上缺陷或改進(jìn)需求，本發(fā)明提供了一種基于關(guān)節(jié)能量均衡分配的關(guān)節(jié)機(jī)器人軌跡規(guī)劃方法，其目的在于通過動(dòng)力學(xué)計(jì)算不同位姿、速度下機(jī)器人的關(guān)節(jié)力矩，建立機(jī)器人關(guān)節(jié)電機(jī)能量均衡的評(píng)價(jià)函數(shù)，使機(jī)器人通過動(dòng)態(tài)粒子群算法自動(dòng)規(guī)劃運(yùn)動(dòng)軌跡，從而優(yōu)化器人運(yùn)動(dòng)過程中各關(guān)節(jié)電機(jī)負(fù)載不均衡，能耗過大的問題。為實(shí)現(xiàn)上述目的，按照本發(fā)明，提供了一種基于關(guān)節(jié)能量均衡分配的關(guān)節(jié)機(jī)器人軌跡規(guī)劃方法，其特征在于，包括以下步驟：1)根據(jù)d-h參數(shù)法建立n關(guān)節(jié)機(jī)器人連桿模型；2)建立機(jī)器人動(dòng)力學(xué)模型，獲得機(jī)器人關(guān)節(jié)力矩關(guān)系式；3)辨識(shí)機(jī)器人在不同位姿的動(dòng)力學(xué)參數(shù)；4)輸入機(jī)器人起始關(guān)節(jié)角度和終止關(guān)節(jié)角度，得到各個(gè)關(guān)節(jié)的力矩曲線，對(duì)機(jī)器人各個(gè)關(guān)節(jié)力矩曲線進(jìn)行擬合，利用曲率最大法挑選特征點(diǎn)，獲得各關(guān)節(jié)對(duì)應(yīng)的特征點(diǎn)數(shù)，記錄最大特征點(diǎn)數(shù)及獲得最大特征點(diǎn)數(shù)的關(guān)節(jié)；5)建立評(píng)價(jià)函數(shù)作為能量均衡分配的判斷標(biāo)準(zhǔn)，利用粒子群算法尋求評(píng)價(jià)函數(shù)最優(yōu)解，則獲得機(jī)器人運(yùn)行軌跡。優(yōu)選地，步驟1)具體步驟如下：根據(jù)d-h參數(shù)法建立關(guān)節(jié)機(jī)器人的連桿坐標(biāo)系模型，列出參數(shù)表，定義參數(shù)表中的關(guān)節(jié)角度θ＝[θ1,θ2,...,θn]，分別求解機(jī)器人關(guān)節(jié)i相對(duì)于關(guān)節(jié)i-1的變換矩陣：其中，2≤i≤n，αi-1表示關(guān)節(jié)i的軸線相對(duì)關(guān)節(jié)i-1軸線轉(zhuǎn)動(dòng)的角度，di表示ai-1和ai之間的距離。優(yōu)選地，步驟2)具體過程如下：利用牛頓-歐拉法，首先向外遞推計(jì)算各連桿的速度和加速度，從而獲得各連桿的慣性力和力矩，然后向內(nèi)遞推計(jì)算各連桿相互作用的力和力矩，以及計(jì)算各關(guān)節(jié)的驅(qū)動(dòng)力和驅(qū)動(dòng)力矩，最終得到機(jī)器人關(guān)節(jié)力矩τ、角度、角速度及角加速度的關(guān)系式：其中，表示慣性力矩，表示離心力矩與哥氏力矩之和，g(θ)表示重力力矩，機(jī)器人關(guān)節(jié)力矩τ是上述力矩的合成。優(yōu)選地，步驟3)具體過程如下：根據(jù)理論辨識(shí)法對(duì)機(jī)器人不同位置及姿態(tài)下各個(gè)連桿的慣性張量進(jìn)行辨識(shí)，慣性張量表示如下所示：其中，ixx＝∫∫∫v(y2+z2)ρdυ、iyy＝∫∫∫v(x2+z2)ρdυ、izz＝∫∫∫v(x2+y2)ρdυ表示機(jī)器人各連桿繞各自連桿坐標(biāo)軸x、y、z的質(zhì)量慣性矩，ixy＝∫∫∫vxyρdυ、iyz＝∫∫∫vyzρdυ、izx＝∫∫∫vzxρdυ表示慣性積。優(yōu)選地，步驟4)具體過程如下：4.1)輸入機(jī)器人起始關(guān)節(jié)角度及目標(biāo)關(guān)節(jié)角度，對(duì)起始位置到目標(biāo)位置的路徑進(jìn)行初始規(guī)劃，使機(jī)器人各關(guān)節(jié)同時(shí)啟動(dòng)，同時(shí)到達(dá)，并以s型加減速運(yùn)行，使機(jī)器人運(yùn)行平穩(wěn)且運(yùn)行時(shí)間最優(yōu)，辨識(shí)在對(duì)應(yīng)位姿下機(jī)器人的慣性特性后，帶入步驟2)中機(jī)器人關(guān)節(jié)力矩的關(guān)系式中，從而得到各關(guān)節(jié)對(duì)應(yīng)的力矩曲線；4.2)利用曲率最大點(diǎn)的方法確定需要插入的中間離散點(diǎn)，中間離散點(diǎn)的曲率計(jì)算如下：并且其中，|κir|表示機(jī)器人關(guān)節(jié)i上第r個(gè)離散點(diǎn)pir所對(duì)應(yīng)的曲率，βir機(jī)器人關(guān)節(jié)i上r-1、r、r+1離散點(diǎn)相連而得到的夾角，||pir+1-pir-1||表示r-1和r+1離散點(diǎn)連線的模長(zhǎng)，pir-1、pir和pir+1分別表示離散點(diǎn)坐標(biāo)(xir-1,τir-1)、(xir,τir)和(xir+1,τir+1)，xir-1、xir和xir+1表示第r-1、r和r+1個(gè)離散點(diǎn),τir-1、τir和τir+1表示第r-1、r和r+1個(gè)離散點(diǎn)的力矩。如果κir＞κir-1、κir＞κir+1和κir＞a·κiavg這三個(gè)條件同時(shí)滿足，則在各關(guān)節(jié)中挑選滿足上述條件的中間離散點(diǎn)作為插值點(diǎn)，遍歷各關(guān)節(jié)，得到各個(gè)關(guān)節(jié)的插值點(diǎn)數(shù)ni，從而獲得特征插值點(diǎn)數(shù)ne＝max(n1,n2,...,nn)，并記錄特征插值點(diǎn)數(shù)所對(duì)應(yīng)關(guān)節(jié)e。優(yōu)選地，步驟5)具體過程如下：5.1)針對(duì)機(jī)器人能量均衡的目標(biāo)建立評(píng)價(jià)函數(shù)f(p):其中，pi＝ti·ωi，pe＝[pe1,pe2,...,pen]表示各關(guān)節(jié)電機(jī)的額定功率，ci對(duì)應(yīng)關(guān)節(jié)i的功率權(quán)重；5.2)對(duì)于關(guān)節(jié)e，以記錄的ne個(gè)特征點(diǎn)為已知關(guān)節(jié)角度xie，規(guī)劃關(guān)節(jié)角速度分別由粒子群算法計(jì)算對(duì)應(yīng)θe下，其他五個(gè)關(guān)節(jié)最優(yōu)角度，具體步驟如下：5.2.1)初始化粒子規(guī)模l，每個(gè)粒子在空間中位置表示為粒子速度表示為其中l(wèi)∈[1,l]，表示第l個(gè)粒子，j∈[1,ne]表示離散點(diǎn)j，k表示當(dāng)前的迭代次數(shù)；5.2.2)計(jì)算第l個(gè)粒子在k+1次迭代后的速度和位置通過第l個(gè)粒子在經(jīng)過k次迭代后的速度和位置及第k次迭代后該粒子自己搜索的歷史最優(yōu)值和全部粒子搜索的歷史最優(yōu)值得到，計(jì)算公式如下所示：其中，ω是慣性系數(shù)，是粒子保持原有速度的系數(shù)，c1是粒子跟蹤自己歷史最優(yōu)值的權(quán)重系數(shù)，c2是粒子跟蹤群體最優(yōu)值的權(quán)重系數(shù)，ξ,η∈[0,1]，代表隨機(jī)數(shù)，r代表約束因子；5.2.3)搜索得到關(guān)節(jié)e角度為θej時(shí)，對(duì)應(yīng)一組機(jī)器人關(guān)節(jié)角，縮小機(jī)器人角度范圍后，再對(duì)θej+1利用粒子群算法得到一組機(jī)器人關(guān)節(jié)角，直到ne個(gè)特征點(diǎn)都得到一組最優(yōu)關(guān)節(jié)值。優(yōu)選地，n＝5或n＝6?？傮w而言，通過本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，能夠取得下列有益效果：1)在機(jī)器人軌跡規(guī)劃過程中，將動(dòng)力學(xué)作為主要判定依據(jù)，能夠?qū)崿F(xiàn)機(jī)器人的動(dòng)態(tài)控制與高精度控制。2)機(jī)器人根據(jù)運(yùn)動(dòng)過程中關(guān)節(jié)所需力矩值，自主搜索運(yùn)動(dòng)軌跡，可以保證機(jī)器人運(yùn)動(dòng)過程中關(guān)節(jié)能量均衡分配，并且所需要的力矩值更優(yōu)，節(jié)約機(jī)器人能耗。附圖說明圖1是華中數(shù)控6008型機(jī)器人的結(jié)構(gòu)圖；圖2是本發(fā)明實(shí)施的求解流程圖。具體實(shí)施方式為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個(gè)實(shí)施方式中所涉及到的技術(shù)特征只要彼此之間未構(gòu)成沖突就可以相互組合。參照?qǐng)D1～圖2，以華中數(shù)控6008機(jī)器人為例，機(jī)器人結(jié)構(gòu)如圖1所示。具體實(shí)施步驟如下所示：s1：利用d-h參數(shù)法建立n關(guān)節(jié)機(jī)器人連桿坐標(biāo)系，得到機(jī)器人連桿參數(shù)如表1所示，計(jì)算機(jī)器人位姿矩陣，該機(jī)器人為6關(guān)節(jié)機(jī)器人，則n＝6；表1機(jī)器人連桿參數(shù)表s2：以角度，角速度及角加速度為輸入量，建立機(jī)器人動(dòng)力學(xué)模型。利用牛頓—?dú)W拉法正向遞推相對(duì)角速度相對(duì)角加速度相對(duì)加速度連桿質(zhì)心加速度連桿質(zhì)心力矩及連桿質(zhì)心力矩然后反向遞推關(guān)節(jié)受力fii及關(guān)節(jié)力矩關(guān)節(jié)力矩表示為：式中表示坐標(biāo)系i+1相對(duì)于坐標(biāo)系i的旋轉(zhuǎn)矩陣，表示坐標(biāo)系i+1相對(duì)于坐標(biāo)系i的位置矩陣。s3：辨識(shí)機(jī)器人處于不同位姿所對(duì)應(yīng)的慣性張量：其中，ixx＝∫∫∫v(y2+z2)ρdυ、iyy＝∫∫∫v(x2+z2)ρdυ、izz＝∫∫∫v(x2+y2)ρdυ表示機(jī)器人各連桿繞各自連桿坐標(biāo)軸x、y、z的質(zhì)量慣性矩，ixy＝∫∫∫vxyρdυ、iyz＝∫∫∫vyzρdυ、izx＝∫∫∫vzxρdυ表示慣性積。用辨識(shí)得到的時(shí)變慣性張量，代替動(dòng)力學(xué)模型中的靜態(tài)慣性張量，進(jìn)行關(guān)節(jié)力矩的計(jì)算。s4：以θ＝[0°,90°,0°,0°,-90°,0°]為機(jī)器人起始點(diǎn)關(guān)節(jié)角度，θ＝[60°,120°,-60°,30°,-30°,0°]為機(jī)器人終止點(diǎn)關(guān)節(jié)角度，對(duì)機(jī)器人進(jìn)行軌跡規(guī)劃，六個(gè)關(guān)節(jié)同時(shí)啟動(dòng)，同時(shí)到達(dá)，得到一組機(jī)器人關(guān)節(jié)力矩曲線。s5：根據(jù)力矩曲線計(jì)算插值點(diǎn)數(shù)，計(jì)算離散點(diǎn)的曲率，選擇曲率最大、產(chǎn)生突變的離散點(diǎn)作為插值點(diǎn)，曲率計(jì)算如下所示：并且其中，|κir|表示機(jī)器人關(guān)節(jié)i上第r個(gè)離散點(diǎn)pir所對(duì)應(yīng)的曲率，βir機(jī)器人關(guān)節(jié)i對(duì)應(yīng)擬合曲線上r-1、r、r+1離散點(diǎn)相連而得到的夾角，||pir+1-pir-1||表示r-1和r+1離散點(diǎn)連線的模長(zhǎng)，pir-1、pir和pir+1分別表示離散點(diǎn)坐標(biāo)(xir-1,τir-1)、(xir,τir)和(xir+1,τir+1)。如果同時(shí)滿足κir＞κir-1、κir＞κir+1和κir＞a·κiavg，則挑選成為特征點(diǎn)，得到特征點(diǎn)數(shù)ne＝max(n1,n2,n3,n4,n5,n6)，并記錄最大特征點(diǎn)數(shù)所對(duì)應(yīng)關(guān)節(jié)e。s6：針對(duì)機(jī)器人能量均衡的目標(biāo)建立評(píng)價(jià)函數(shù)，機(jī)器人各關(guān)節(jié)的質(zhì)量值如表2所示。表2機(jī)器人各關(guān)節(jié)參數(shù)表imi(kg)cmi(mm)pei(kw)189.303(-84.845,-4.973,-127.436)1220.121(95.384,90,27.670)2.5326.074(75.335,-60.73,-6.994)148.084(0.066,-67.6,105.307)0.252.082(-0.26,63.63,-20.5)0.260.04(0,0,38.25)0.1根據(jù)表2，可得6008機(jī)器人關(guān)節(jié)電機(jī)額定功率pe＝[1,2.5,1,0.2,0.2,0.1]，取c1＝c2＝c3＝0.8，c4＝c5＝c6＝0.2，可得評(píng)價(jià)函數(shù)f(p):其中，pi＝ti·ωi，s7：對(duì)六個(gè)關(guān)節(jié)角進(jìn)行插點(diǎn)，插入點(diǎn)數(shù)為ne，以關(guān)節(jié)e為標(biāo)準(zhǔn)，按曲率極值點(diǎn)分段，利用粒子群法計(jì)算其他五個(gè)關(guān)節(jié)在各段所對(duì)應(yīng)的關(guān)節(jié)角度。s8：粒子群算法中粒子數(shù)過小可能導(dǎo)致收斂到局部，粒子數(shù)過大會(huì)使時(shí)間過長(zhǎng)，取粒子規(guī)模l＝24，對(duì)6008機(jī)器人除關(guān)節(jié)e外五個(gè)關(guān)節(jié)有五個(gè)維度，第l個(gè)粒子在經(jīng)過k次迭代后在空間中位置表示為粒子速度表示為s9:第l個(gè)粒子在k+1次迭代后的速度和位置可以用下列公式計(jì)算：其中，表示第l個(gè)粒子的速度和位置，表示k次迭代后該粒子自己搜索的歷史最優(yōu)值，表示全部粒子搜索的歷史最優(yōu)值，ξ,η∈[0,1]，代表0到1間均勻隨機(jī)數(shù)。s10：對(duì)關(guān)節(jié)e，每個(gè)角度值都對(duì)應(yīng)一組解，從第一個(gè)插值點(diǎn)開始，求得一組解后縮小機(jī)器人角度范圍，再利用粒子群算法求下一個(gè)插值點(diǎn)機(jī)器人關(guān)節(jié)角度，知道ne個(gè)特征點(diǎn)都求得對(duì)應(yīng)的最優(yōu)關(guān)節(jié)值。本發(fā)明求得的機(jī)器人軌跡是以關(guān)節(jié)能量均衡分配為目標(biāo)，同時(shí)采用s加減速對(duì)求得插值點(diǎn)進(jìn)行插值，使機(jī)器人運(yùn)動(dòng)時(shí)間也能保證最優(yōu)，速度，加速度連續(xù)變化，運(yùn)動(dòng)軌跡平滑。本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。當(dāng)前第1頁(yè)12