一種基于fpga的六軸工業(yè)機(jī)器人測(cè)速方法
【專利摘要】本發(fā)明公開了一種基于FPGA的六軸工業(yè)機(jī)器人測(cè)速方法，該方法包括電機(jī)速度測(cè)量和多通道除法器的實(shí)現(xiàn)；電機(jī)速度測(cè)量采用改進(jìn)的M法和T法綜合的方法,避免了高速和低速臨界速度的人為定義，提高了測(cè)度的測(cè)量精度；并且多通道除法器的實(shí)現(xiàn)采用了多通道除法器計(jì)算各軸速度，使得六軸工業(yè)機(jī)器人多軸共享一個(gè)除法器資源，在節(jié)約大量LE邏輯資源的同時(shí)，并不影響各軸間的獨(dú)立性。
【專利說明】
一種基于FPGA的六軸工業(yè)機(jī)器人測(cè)速方法
技術(shù)領(lǐng)域
[0001]本發(fā)明屬于工業(yè)機(jī)器人控制領(lǐng)域，具體涉及到一種基于FPGA的六軸工業(yè)機(jī)器人測(cè)速方法。
【背景技術(shù)】
[0002]提高速度測(cè)量精度是提高工業(yè)機(jī)器人控制精度的一個(gè)重要方向，傳統(tǒng)的電機(jī)測(cè)速方法有M法和T法:M法是測(cè)量一定時(shí)間內(nèi)脈沖的個(gè)數(shù)，也就是一定時(shí)間內(nèi)電機(jī)旋轉(zhuǎn)過的角度，從而得到角速度，當(dāng)一定時(shí)間內(nèi)脈沖個(gè)數(shù)越多時(shí)，測(cè)量的結(jié)果越精確，所以此方法適用于高速情況;T法是測(cè)量?jī)蓚€(gè)脈沖間的時(shí)間間隔，轉(zhuǎn)速越高，時(shí)間間隔越小，轉(zhuǎn)速越慢，時(shí)間間隔越長，所以此方法用于測(cè)量慢速情況;工業(yè)上一般綜合M法和T法來提高電機(jī)測(cè)速精度，但是這種方法高速和低速的分界不是很明確，往往人為來定義臨界速度值，給速度測(cè)量帶來了極大地不便，并且也帶來了測(cè)速誤差?；贔PGA的六軸工業(yè)機(jī)器人，為了保證各軸間互不影響，提高軸間同步度，速度測(cè)量模塊需要硬件描述語言例化產(chǎn)生;但是在速度計(jì)算中，需要用到六個(gè)除法器，除法器需要耗費(fèi)大量的LE資源，這給有限LE資源的FPGA帶來了巨大壓力。

【發(fā)明內(nèi)容】

[0003]本發(fā)明針對(duì)現(xiàn)有技術(shù)的不足，提出了一種基于FPGA的六軸工業(yè)機(jī)器人測(cè)速方法，包括電機(jī)測(cè)速方法實(shí)現(xiàn)步驟和多通道除法器實(shí)現(xiàn)步驟兩部分。
[0004]電機(jī)速度測(cè)量實(shí)現(xiàn)步驟包括:
[0005]步驟一:電機(jī)速度測(cè)量
[0006]①若在50us定時(shí)器內(nèi)出現(xiàn)M個(gè)脈沖，M2 2，并且捕捉到第一個(gè)脈沖的上升沿到第M個(gè)脈沖的上升沿時(shí)間為T，則速度=M*脈沖當(dāng)量/T;②若在50us定時(shí)器內(nèi)出現(xiàn)的脈沖數(shù)M，M
<I，則繼續(xù)等待;③若在50us定時(shí)器外出現(xiàn)兩個(gè)脈沖，且捕捉到兩個(gè)脈沖間的時(shí)間為TJlJ速度=脈沖當(dāng)量/T;④若在50us定時(shí)器外出現(xiàn)一個(gè)脈沖或者零個(gè)脈沖，認(rèn)為此時(shí)的速度為O;所述電機(jī)速度測(cè)量實(shí)現(xiàn)的①和③均出現(xiàn)了 1/T，若每個(gè)軸均使用一個(gè)除法器，需要耗費(fèi)大量的LE資源，為了實(shí)現(xiàn)多軸邏輯共享同一除法器，可以采用時(shí)分復(fù)用的辦法。又由于只需要計(jì)算速度，故除法器的被除數(shù)可以固定為一個(gè)常數(shù)；
[0007]除法器的邏輯直接調(diào)用FPGA編譯器中現(xiàn)成的LPM_DIVIDE模塊，多通道實(shí)現(xiàn)則是自行設(shè)計(jì)的定序器邏輯。
[0008]步驟二:多通道除法器的實(shí)現(xiàn)
[0009]當(dāng)出現(xiàn)①和③的時(shí)候；
[0010]步驟1、用一個(gè)最大計(jì)數(shù)值為除法器流水線級(jí)數(shù)的循環(huán)自增計(jì)數(shù)器；
[0011]步驟2、當(dāng)某個(gè)通道需要使用除法器時(shí)，首先將該通道的除數(shù)值存入暫存器中，然后將定序器中對(duì)應(yīng)通道的標(biāo)志位置為I;
[0012]步驟3、假如檢測(cè)到定序器中所有通道的標(biāo)志位為I的，就按照固定的優(yōu)先級(jí)別順序，將優(yōu)先級(jí)最高通道的除數(shù)暫存器的值賦給除法器的除數(shù)，同時(shí)記錄該時(shí)刻自增計(jì)數(shù)器的值，然后在計(jì)數(shù)器在接下來的一輪自增計(jì)數(shù)計(jì)到此數(shù)值時(shí)，依據(jù)分選器取出計(jì)算完的商。
[0013]與現(xiàn)有的技術(shù)相比，本發(fā)明的有益效果是:采用改進(jìn)的M法和T法綜合的方法，避免了高速和低速臨界速度的人為定義，提高了測(cè)度的測(cè)量精度;并且采用了多通道除法器計(jì)算各軸速度，使得六軸工業(yè)機(jī)器人多軸共享一個(gè)除法器資源，在節(jié)約大量LE邏輯資源的同時(shí)，并不影響各軸間的獨(dú)立性。
【附圖說明】
[0014]圖1是六通道復(fù)用除法器簡(jiǎn)化結(jié)構(gòu)圖
【具體實(shí)施方式】
[0015]一種基于FPGA的六軸工業(yè)機(jī)器人測(cè)速方法，包括電機(jī)測(cè)速方法實(shí)現(xiàn)步驟和多通道除法器實(shí)現(xiàn)步驟兩部分。電機(jī)速度測(cè)量實(shí)現(xiàn)步驟包括:
[0016]步驟一:電機(jī)速度測(cè)量
[0017]①若在50us定時(shí)器內(nèi)出現(xiàn)M個(gè)脈沖，M2 2，并且捕捉到第一個(gè)脈沖的上升沿到第M個(gè)脈沖的上升沿時(shí)間為T，則速度=M*脈沖當(dāng)量/T;②若在50us定時(shí)器內(nèi)出現(xiàn)的脈沖數(shù)M，M
<I，則繼續(xù)等待;③若在50us定時(shí)器外出現(xiàn)兩個(gè)脈沖，且捕捉到兩個(gè)脈沖間的時(shí)間為TJlJ速度=脈沖當(dāng)量/T;④若在50us定時(shí)器外出現(xiàn)一個(gè)脈沖或者零個(gè)脈沖，認(rèn)為此時(shí)的速度為O;所述電機(jī)速度測(cè)量實(shí)現(xiàn)的①和③均出現(xiàn)了 1/T，若每個(gè)軸均使用一個(gè)除法器，需要耗費(fèi)大量的LE資源，為了實(shí)現(xiàn)多軸邏輯共享同一除法器，可以采用時(shí)分復(fù)用的辦法。又由于只需要計(jì)算速度，故除法器的被除數(shù)可以固定為一個(gè)常數(shù);如圖1所示；
[0018]除法器的邏輯直接調(diào)用FPGA編譯器中現(xiàn)成的LPM_DIVIDE模塊，多通道實(shí)現(xiàn)則是自行設(shè)計(jì)的定序器邏輯。
[0019]步驟二:多通道除法器的實(shí)現(xiàn)
[0020]當(dāng)出現(xiàn)①和③的時(shí)候；
[0021 ]步驟1、用一個(gè)最大計(jì)數(shù)值為除法器流水線級(jí)數(shù)的循環(huán)自增計(jì)數(shù)器；
[0022]步驟2、當(dāng)某個(gè)通道需要使用除法器時(shí)，首先將該通道的除數(shù)值存入暫存器中，然后將定序器中對(duì)應(yīng)通道的標(biāo)志位置為I;
[0023]步驟3、假如檢測(cè)到定序器中所有通道的標(biāo)志位為I的，就按照固定的優(yōu)先級(jí)別順序，將優(yōu)先級(jí)最高通道的除數(shù)暫存器的值賦給除法器的除數(shù)，同時(shí)記錄該時(shí)刻自增計(jì)數(shù)器的值，然后在計(jì)數(shù)器在接下來的一輪自增計(jì)數(shù)計(jì)到此數(shù)值時(shí)，依據(jù)分選器取出計(jì)算完的商。
【主權(quán)項(xiàng)】
1.一種基于FPGA的六軸工業(yè)機(jī)器人測(cè)速方法，其特征在于: 步驟一:電機(jī)速度測(cè)量 ①若在50us定時(shí)器內(nèi)出現(xiàn)M個(gè)脈沖，M 2 2，并且捕捉到第一個(gè)脈沖的上升沿到第M個(gè)脈沖的上升沿時(shí)間為T，則速度=M*脈沖當(dāng)量/T;②若在50us定時(shí)器內(nèi)出現(xiàn)的脈沖數(shù)M，M< 1，則繼續(xù)等待;③若在50us定時(shí)器外出現(xiàn)兩個(gè)脈沖，且捕捉到兩個(gè)脈沖間的時(shí)間為T，則速度=脈沖當(dāng)量/T;④若在50us定時(shí)器外出現(xiàn)一個(gè)脈沖或者零個(gè)脈沖，認(rèn)為此時(shí)的速度為O;步驟二:多通道除法器的實(shí)現(xiàn)當(dāng)出現(xiàn)①和③的時(shí)候； 步驟1、用一個(gè)最大計(jì)數(shù)值為除法器流水線級(jí)數(shù)的循環(huán)自增計(jì)數(shù)器； 步驟2、當(dāng)某個(gè)通道需要使用除法器時(shí)，首先將該通道的除數(shù)值存入暫存器中，然后將定序器中對(duì)應(yīng)通道的標(biāo)志位置為I; 步驟3、假如檢測(cè)到定序器中所有通道的標(biāo)志位為I的，就按照固定的優(yōu)先級(jí)別順序，將優(yōu)先級(jí)最高通道的除數(shù)暫存器的值賦給除法器的除數(shù)，同時(shí)記錄該時(shí)刻自增計(jì)數(shù)器的值，然后在計(jì)數(shù)器在接下來的一輪自增計(jì)數(shù)計(jì)到此數(shù)值時(shí)，依據(jù)分選器取出計(jì)算完的商。
【文檔編號(hào)】B25J9/16GK105856228SQ201610290284
【公開日】2016年8月17日
【申請(qǐng)日】2016年5月3日
【發(fā)明人】高明煜, 劉雷, 黃繼業(yè), 龔文科, 曾毓, 何志偉, 王協(xié)平
【申請(qǐng)人】杭州電子科技大學(xué)