本(ben)發明屬于(yu)工業(ye)機(ji)器人(ren)技術領域，涉(she)及(ji)一種工業(ye)機(ji)器人(ren)桿長的(de)校正(zheng)方法以及(ji)校正(zheng)工具(ju)，該方法可應用于(yu)六軸工業(ye)機(ji)器人(ren)研發、測試、現場使(shi)用等多個過程中。

背景技術：

在工業機(ji)器人的研發(fa)過程中，由(you)于其各(ge)部件(jian)的材料、制造工藝、安(an)裝(zhuang)等原因，會導(dao)致(zhi)工業機(ji)器人各(ge)關節的桿長(chang)發(fa)生變化。但是，機(ji)器人在進行軌跡控制時卻(que)一直以工業機(ji)器人理論(lun)上(shang)的建模的桿長(chang)來(lai)進行規劃(hua)，實(shi)際桿長(chang)的變化會導(dao)致(zhi)工業機(ji)器人在定位精度與(yu)重(zhong)復精度上(shang)產(chan)生較大誤差(cha)。

在(zai)工業生產過程中，桿長的變(bian)化，輕則(ze)導致(zhi)(zhi)產品質量一致(zhi)(zhi)性不好，產品不合格率(lv)提(ti)高，重(zhong)則(ze)導致(zhi)(zhi)生產線(xian)上的工件發生嚴重(zhong)干涉(she)，造成設備(bei)甚至人員損傷。

通(tong)過(guo)對(dui)桿(gan)長進行實(shi)際(ji)測量，對(dui)控制(zhi)系統中桿(gan)長參(can)數的定時檢測及修正(zheng)，可以使(shi)工業機器(qi)人(ren)(ren)控制(zhi)軌跡得到補(bu)償，提高產品一致性要求，也能定時地了(le)解工業機器(qi)人(ren)(ren)的本(ben)體情況，及早通(tong)知維護人(ren)(ren)員介(jie)入(ru)，避免(mian)工廠事故的發生。

目前工(gong)業機器(qi)(qi)人(ren)(ren)廠(chang)家較少(shao)會對桿(gan)(gan)長(chang)參(can)數(shu)進行校正(zheng)，少(shao)部(bu)(bu)(bu)分(fen)廠(chang)家會在產品出廠(chang)時使用三坐(zuo)標(biao)儀、激光測(ce)(ce)量(liang)(liang)儀等工(gong)具進行外(wai)(wai)部(bu)(bu)(bu)桿(gan)(gan)長(chang)參(can)數(shu)測(ce)(ce)量(liang)(liang)并修正(zheng)。一方(fang)面(mian)，此方(fang)式采用第三方(fang)測(ce)(ce)量(liang)(liang)工(gong)具是從機器(qi)(qi)人(ren)(ren)外(wai)(wai)部(bu)(bu)(bu)得到機器(qi)(qi)人(ren)(ren)桿(gan)(gan)長(chang)參(can)數(shu)，對機器(qi)(qi)人(ren)(ren)本(ben)體測(ce)(ce)量(liang)(liang)點(dian)的選(xuan)定及檢(jian)測(ce)(ce)人(ren)(ren)員(yuan)的操作技術(shu)有著較高要(yao)求；另一方(fang)面(mian)，第三方(fang)的測(ce)(ce)量(liang)(liang)工(gong)具其價格昂貴，采購(gou)周期長(chang)，對于處于初級研發(fa)階段的或中小型的機器(qi)(qi)人(ren)(ren)公司不(bu)易實現(xian)。

本申請基(ji)于工業(ye)機器人(ren)的運動(dong)學、矩陣運算、病態矩陣處(chu)理、矩陣降(jiang)階處(chu)理和最小二乘法計算等原(yuan)理解(jie)決了工業(ye)機器人(ren)桿長參數(shu)的校正問題。

技術實現要素：

本發明的目(mu)的是(shi)提供(gong)一種工業機(ji)(ji)器(qi)人桿長參(can)數的校正方法，可用(yong)于六軸工業機(ji)(ji)器(qi)人桿長參(can)數的計算與修(xiu)正，其解決(jue)了六軸工業機(ji)(ji)器(qi)人各部件在生(sheng)產、制造、安裝(zhuang)及使(shi)用(yong)過程(cheng)中產生(sheng)的機(ji)(ji)器(qi)人桿長變化而導致的定位(wei)精(jing)度及重復(fu)精(jing)度誤差的問題。

此外，本發(fa)明(ming)的(de)另一(yi)目(mu)的(de)是提供一(yi)種工(gong)業機器人桿長參數的(de)校正工(gong)具。

本(ben)發明的技術方案是：

本發(fa)明的一個技(ji)術方(fang)案是提供(gong)一種工業(ye)機器人(ren)桿(gan)長參數(shu)的校正工具(ju)，包括(kuo)(kuo)有(you)臺(tai)(tai)(tai)階板和釘盤(pan)，所述臺(tai)(tai)(tai)階板上設(she)有(you)多個同高度的臺(tai)(tai)(tai)階，在每(mei)個臺(tai)(tai)(tai)階上設(she)有(you)若干(gan)個圓孔(kong)，所有(you)的圓孔(kong)構成矩陣排(pai)布設(she)置；所述釘盤(pan)包括(kuo)(kuo)法蘭部和桿(gan)部，在桿(gan)部的末端(duan)加(jia)工成與圓孔(kong)相配(pei)合的細桿(gan)部。

在該技術方(fang)案的(de)一個(ge)實(shi)施例中(zhong)，所(suo)(suo)述臺階(jie)(jie)板的(de)臺階(jie)(jie)數(shu)為四層(ceng)，每層(ceng)臺階(jie)(jie)上設有(you)四個(ge)圓(yuan)孔，所(suo)(suo)有(you)的(de)圓(yuan)孔構成4×4矩(ju)陣(zhen)排布設置。

在該技術(shu)方案(an)的(de)一個(ge)實(shi)施例(li)中，所(suo)述釘盤的(de)桿(gan)部的(de)軸線(xian)(xian)與法蘭(lan)的(de)軸線(xian)(xian)重合，所(suo)述法蘭(lan)上設(she)有(you)若干(gan)個(ge)用(yong)以安裝在工業機器人(ren)的(de)末端法蘭(lan)的(de)螺孔。

本發明的另一技(ji)術方(fang)(fang)案是(shi)提供一種工業機器人桿長參數的校正(zheng)方(fang)(fang)法，該(gai)方(fang)(fang)法是(shi)基(ji)于上述技(ji)術方(fang)(fang)案的校正(zheng)工具的基(ji)礎(chu)上實現的，該(gai)方(fang)(fang)法包(bao)括如下步驟：

s1、安裝臺階板(ban)和釘盤，并對工業機器(qi)人進行建(jian)模原點對位；

s2、控制工業機器人使末端的(de)釘(ding)盤移動(dong)到臺階板其中一個圓孔附近；

s3、控制釘(ding)盤的細桿部完全進入(ru)圓(yuan)孔，并(bing)記錄在當前圓(yuan)孔位置時的關(guan)節數據；

s4、重復上述s2與s3的(de)步驟，依次(ci)完(wan)成其它圓孔(kong)位(wei)置的(de)校正，并記錄在每個圓孔(kong)位(wei)置時相(xiang)應的(de)關節數據；

s5、軟(ruan)件計算得出實際測量桿(gan)長(chang)的數值，并(bing)修正到機器人的桿(gan)長(chang)參數上。

在一種(zhong)實施例(li)中，所述(shu)s1步驟中安裝臺階(jie)板和釘(ding)盤，具體包括步驟：

s11、安裝時將臺階板外沿其中(zhong)的(de)一邊垂直于工業機器人的(de)世界坐(zuo)標系的(de)x軸(zhou)，并與(yu)y軸(zhou)成30度角。

s12、將釘盤(pan)安裝于工業機(ji)器人的末端法蘭上。

在一種實施例(li)中(zhong)，所述s3步驟中(zhong)的記(ji)錄在當前圓(yuan)孔位(wei)置時的關節數據，圓(yuan)孔是以孔的底面(mian)的圓(yuan)心作為圓(yuan)孔的測量(liang)點(dian)，并以測量(liang)點(dian)為基礎測量(liang)關節數據。

在(zai)一種實施例中，所述s3步驟中的(de)(de)記錄(lu)在(zai)當前圓(yuan)孔位置(zhi)(zhi)時(shi)的(de)(de)關節數(shu)(shu)據，所述關節數(shu)(shu)據包括(kuo)工(gong)業機(ji)器人的(de)(de)各個關節的(de)(de)夾角(jiao)，該夾角(jiao)是通過設置(zhi)(zhi)于關節處的(de)(de)角(jiao)度傳感器進(jin)行測量的(de)(de)。

在一種實施例中，在軟件計算(suan)得出實際測量桿長的(de)數值時采用(yong)以下的(de)公式：

px＝l6sθ5sθ4sθ1+cθ1(l2+l3cθ2+c(θ2+θ3)(l4+l6cθ4sθ5)+(l5+l6cθ5)s(θ2+θ3))(公式(shi)1)

py＝-l6cθ1sθ5sθ4+sθ1(l2+l3cθ2+c(θ2+θ3)(l4+l6cθ4sθ5)+(l5+l6cθ5)s(θ2+θ3))(公式(shi)2)

pz＝l1-(l5+l6cθ5)c(θ2+θ3)+l3sθ2+(l4+l6cθ4sθ5)s(θ2+θ3))(公(gong)式3)

其(qi)中：px是(shi)指臺階(jie)板上圓孔位置(zhi)的(de)測(ce)量點相對于工(gong)業(ye)機器人(ren)的(de)世界坐標系的(de)沿x軸的(de)數值(zhi)，py是(shi)指圓孔位置(zhi)的(de)測(ce)量點沿y軸的(de)數值(zhi)，pz是(shi)指圓孔位置(zhi)的(de)測(ce)量點沿z軸的(de)數值(zhi)；

s是(shi)正弦sin的縮(suo)寫(xie)；c是(shi)余弦cos的縮(suo)寫(xie)；θ是(shi)指工業機(ji)器人(ren)的各個(ge)關節的夾角，l是(shi)指工業機(ji)器人(ren)的各個(ge)桿長的數值(zhi)。

在一(yi)種實施例(li)中(zhong)，在軟件計算桿長的數值(zhi)需(xu)要至少三個不同圓孔位置(zhi)的測量點的關節(jie)數據(ju)為基礎，根(gen)據(ju)公式1-3采(cai)用矩陣運算桿長的數值(zhi)。

該技術方案的(de)(de)(de)校正方法(fa)采用(yong)特制(zhi)的(de)(de)(de)校正工具，指定所(suo)需的(de)(de)(de)圓孔測(ce)量(liang)點序列，使(shi)所(suo)生成的(de)(de)(de)求解機器人的(de)(de)(de)關(guan)(guan)節與桿長的(de)(de)(de)相關(guan)(guan)矩(ju)陣的(de)(de)(de)列向(xiang)量(liang)或行(xing)向(xiang)量(liang)線性(xing)無(wu)關(guan)(guan)，以(yi)保證各關(guan)(guan)節桿長解的(de)(de)(de)唯一(yi)性(xing)和準確性(xing)；采用(yong)廣義最(zui)小二(er)乘法(fa)處理(li)測(ce)量(liang)值序列，可消除(chu)測(ce)量(liang)過程可能引入的(de)(de)(de)各關(guan)(guan)節桿長的(de)(de)(de)離散誤差，提高算法(fa)的(de)(de)(de)精(jing)確度。

附圖說明

圖1是(shi)本發明的(de)校正工具的(de)整體結構視(shi)圖。

圖2是本發明的校正工具的分解視圖。

具體實施方式

以下(xia)結合附(fu)圖與具(ju)體實(shi)施例對本發(fa)明(ming)的技術(shu)方案做進一步詳細(xi)說明(ming)，以便(bian)本領域的技術(shu)人員能清楚完整(zheng)的理解本發(fa)明(ming)中的技術(shu)內容。

實施例一

參(can)照(zhao)圖(tu)1與圖(tu)2所(suo)示，本實(shi)施例(li)是一種工業(ye)機器人桿(gan)長(chang)參(can)數的(de)校正工具，包(bao)括有(you)臺(tai)階(jie)板(ban)和釘盤，臺(tai)階(jie)板(ban)上(shang)設(she)有(you)四層相(xiang)同高度的(de)臺(tai)階(jie)，每(mei)層高度為(wei)10mm，最(zui)下層的(de)長(chang)寬都(dou)為(wei)200mm，在每(mei)個臺(tai)階(jie)上(shang)設(she)有(you)四個圓孔，共(gong)有(you)16個，所(suo)有(you)圓孔構成4×4矩陣排(pai)布設(she)置，圓孔之間距(ju)離為(wei)40mm，圓孔的(de)直(zhi)徑(jing)為(wei)8mm+0.1mm，深度為(wei)10mm，圓孔的(de)設(she)計以便與釘盤的(de)細桿(gan)部相(xiang)配(pei)合，在最(zui)下層的(de)四角還設(she)有(you)四個安裝(zhuang)用螺(luo)孔。

釘盤(pan)(pan)包括法(fa)蘭部(bu)(bu)(bu)、桿(gan)(gan)部(bu)(bu)(bu)和細桿(gan)(gan)部(bu)(bu)(bu)，釘盤(pan)(pan)桿(gan)(gan)部(bu)(bu)(bu)的軸線與(yu)法(fa)蘭的軸線重(zhong)合，桿(gan)(gan)部(bu)(bu)(bu)長(chang)為(wei)100mm，直(zhi)徑為(wei)10mm，法(fa)蘭上設有(you)螺(luo)孔，用以將釘盤(pan)(pan)安(an)裝在(zai)工(gong)業機器人(ren)的末(mo)端法(fa)蘭。在(zai)桿(gan)(gan)部(bu)(bu)(bu)的末(mo)端加工(gong)成與(yu)圓孔相配合的細桿(gan)(gan)部(bu)(bu)(bu)，細桿(gan)(gan)部(bu)(bu)(bu)長(chang)為(wei)10mm，直(zhi)徑為(wei)8mm-0.1mm，并(bing)在(zai)末(mo)端作圓角處理(li)，同(tong)樣(yang)的在(zai)圓孔的入口也(ye)做圓角處理(li)，方(fang)便配合。

實施例二

本(ben)實施(shi)(shi)例是一種工(gong)業機器(qi)人(ren)桿長(chang)參數(shu)的(de)(de)校正方法(fa)，該方法(fa)是基(ji)于實施(shi)(shi)例一中的(de)(de)校正工(gong)具的(de)(de)基(ji)礎上實現的(de)(de)，是對具有六軸工(gong)業機器(qi)人(ren)的(de)(de)桿長(chang)進(jin)行測(ce)量，其(qi)具備六個桿長(chang)和六個關(guan)(guan)節，本(ben)實施(shi)(shi)例中桿和關(guan)(guan)節的(de)(de)編號從靠近基(ji)座一側開始。

該校正方法包括(kuo)如下步驟：

s1、安(an)裝(zhuang)臺階板(ban)和釘盤，并(bing)對工業(ye)機器人(ren)進行建模原(yuan)點對位(wei)，安(an)裝(zhuang)時將臺階板(ban)外(wai)沿其中的(de)一邊垂直于(yu)工業(ye)機器人(ren)的(de)世界(jie)坐標系的(de)x軸(zhou)，并(bing)與y軸(zhou)成30度角(jiao)，將釘盤的(de)法蘭(lan)部固定安(an)裝(zhuang)到工業(ye)機器人(ren)的(de)末端法蘭(lan)上。

s2、對系(xi)統進行設置，包括控制驅動(dong)器開啟(qi)軟(ruan)浮動(dong)功能，限制電流幅值(zhi)，允(yun)許跟蹤位置誤差超限；控制工業機器人使末端的釘盤移動(dong)到臺階板上第(di)一層的左側的圓孔附(fu)近；

s3、控制(zhi)釘盤的(de)細桿部完全進入圓(yuan)孔，并記錄在當前圓(yuan)孔位置時的(de)關(guan)節(jie)數據；

s4、重(zhong)復(fu)上述s2與(yu)s3的(de)步驟，依次完成其它圓(yuan)孔位置的(de)校正，并記(ji)錄在每個圓(yuan)孔位置時(shi)相應的(de)關節(jie)數據；

s5、軟件計算得出實際測量(liang)桿長(chang)的數值，并修正到機(ji)器人(ren)的桿長(chang)參數上。

其(qi)中，圓孔是(shi)以孔的(de)底(di)面的(de)圓心作為圓孔的(de)測(ce)量(liang)點，并(bing)以測(ce)量(liang)點為基礎測(ce)量(liang)關(guan)節(jie)數據，關(guan)節(jie)數據包括工(gong)業機器人的(de)各個關(guan)節(jie)的(de)夾(jia)角(jiao)，該夾(jia)角(jiao)是(shi)通過設置(zhi)于關(guan)節(jie)處的(de)角(jiao)度傳感器進行測(ce)量(liang)的(de)。

以下根據(ju)測量記錄的(de)數據(ju)對桿長(chang)進(jin)行計算的(de)過(guo)程是：

由(you)運動(dong)學正(zheng)解方程可知，在軟件計(ji)算(suan)得出實際測(ce)量桿(gan)長的數值時采用以下(xia)的公式：

px＝l6sθ5sθ4sθ1+cθ1(l2+l3cθ2+c(θ2+θ3)(l4+l6cθ4sθ5)+(l5+l6cθ5)s(θ2+θ3))(公(gong)式1)

py＝-l6cθ1sθ5sθ4+sθ1(l2+l3cθ2+c(θ2+θ3)(l4+l6cθ4sθ5)+(l5+l6cθ5)s(θ2+θ3))(公(gong)式(shi)2)

pz＝l1-(l5+l6cθ5)c(θ2+θ3)+l3sθ2+(l4+l6cθ4sθ5)s(θ2+θ3))(公式(shi)3)

其中：px是(shi)指(zhi)臺階板上(shang)圓孔位(wei)置(zhi)的(de)(de)(de)(de)測量點相對于工(gong)業機(ji)器人的(de)(de)(de)(de)世界坐標系的(de)(de)(de)(de)沿x軸(zhou)的(de)(de)(de)(de)數(shu)值，py是(shi)指(zhi)圓孔位(wei)置(zhi)的(de)(de)(de)(de)測量點沿y軸(zhou)的(de)(de)(de)(de)數(shu)值，pz是(shi)指(zhi)圓孔位(wei)置(zhi)的(de)(de)(de)(de)測量點沿z軸(zhou)的(de)(de)(de)(de)數(shu)值；

s是正(zheng)弦(xian)sin的(de)(de)縮(suo)寫(xie)(xie)；c是余(yu)弦(xian)cos的(de)(de)縮(suo)寫(xie)(xie)；θ是指工(gong)業(ye)機器人的(de)(de)各(ge)個關(guan)節的(de)(de)夾(jia)角，l是指工(gong)業(ye)機器人的(de)(de)各(ge)個桿長的(de)(de)數值。

上述(shu)公式將關(guan)節參數(shu)的(de)系數(shu)分離得到：

px＝(0)l1+(cθ1)l2+(cθ1cθ2)l3+(cθ1c(θ2+θ3))l4+(cθ1s(θ2+θ3))l5+(sθ5sθ4sθ1+cθ1(c(θ2+θ3)cθ4sθ5+s(θ2+θ3)cθ5))l6

(公式4)

py＝(0)l1+(sθ1)l2+(sθ1cθ2)l3+(sθ1c(θ2+θ3))l4+(sθ1s(θ2+θ3))l5+(-cθ1sθ5sθ4+sθ1(c(θ2+θ3)cθ4sθ5+s(θ2+θ3)cθ5))l6

(公式5)

pz＝(1)l1+(0)l2+(sθ2)l3+(s(θ2+θ3))l4+(-c(θ2+θ3))l5+(-c(θ2+θ3)cθ5+s(θ2+θ3)cθ4sθ5)l6

(公式6)

以(yi)兩個測量點為例，分別(bie)標為測量點j和測量點k，根據上(shang)述公式4-6轉為矩陣可得：

其(qi)中，j與k是排列組合中線性(xing)無關的兩個測(ce)量點(dian)序號，pjx是指(zhi)測(ce)量點(dian)j相對于工業機器人的世界(jie)坐標系的沿x軸(zhou)的數(shu)值(zhi)，pjy是指(zhi)測(ce)量點(dian)j沿y軸(zhou)的數(shu)值(zhi)，pjz是指(zhi)測(ce)量點(dian)j沿z軸(zhou)的數(shu)值(zhi)，其(qi)它(ta)測(ce)量點(dian)以此(ci)類推。

gj表示(shi)公式4-6中關(guan)節(jie)參(can)數(shu)系(xi)數(shu)的計算(suan)，使用測(ce)量(liang)第j個測(ce)量(liang)點時的機器人關(guan)節(jie)值(θ1，θ2，θ3，θ4，θ5，θ6)。

定下如下關(guan)節(jie)參數系統(tong)另(ling)外：

gx，1＝0

gx，2＝cθ1

gx，3＝cθ1cθ2

gx，4＝cθ1c(θ2+θ3)

gx，5＝cθ1s(θ2+θ3)

gx，6＝sθ5sθ4sθ1+cθ1(cθ23cθ4sθ5+s(θ2+θ3)cθ5)

gy，1＝0

gy，2＝sθ1

gy，3＝sθ1cθ2

gy，4＝sθ1c(θ2+θ3)

gy，5＝sθ1s(θ2+θ3)

gy，6＝-cθ1sθ5sθ4+sθ1(c(θ2+θ3)cθ4sθ5+s(θ2+θ3)cθ5)

gz，1＝1

gz，2＝0

gz，3＝sθ2

gz，4＝s(θ2+θ3)

gz，5＝-c(θ2+θ3)

gz，6＝-c(θ2+θ3)cθ5+s(θ2+θ3)cθ4sθ5

當得(de)知兩個(ge)測量點(dian)位置(zhi)及相應系數矩陣時，即(ji)機器人關節值(θ1，θ2，θ3，θ4，θ5，θ6)。則公(gong)式(shi)7可簡化為各桿長的(de)公(gong)式(shi)如下：

對于(yu)多個(ge)測量(liang)點，如果只取(qu)測量(liang)點的(de)x與z軸位(wei)置坐標的(de)p值時，根據公(gong)(gong)式(shi)8推導可得各桿長的(de)公(gong)(gong)式(shi)為：

更進(jin)一(yi)步地，對(dui)于(yu)一(yi)個測量點(dian)形(xing)成的2行6列系數矩陣如下(xia)：

如(ru)果存在m個(ge)測(ce)量點，則可形成(cheng)2m行6列系數矩陣如(ru)下：

對矩陣方程公式10進行(xing)處(chu)理得：

對(dui)其關(guan)節參(can)數求解，可得(de)：

在得(de)到每個測量點(dian)的關節值后(hou)，通過計算軟件進行編(bian)程計算即可得(de)到各桿長的數值。

以上實施(shi)例為本發(fa)(fa)明的(de)優(you)選(xuan)實施(shi)例，本發(fa)(fa)明不限于(yu)(yu)上述實施(shi)例，對于(yu)(yu)本領域普通(tong)技(ji)術人員而言，在不背(bei)離(li)本發(fa)(fa)明技(ji)術原理的(de)基(ji)礎上所(suo)做的(de)任何顯而易見(jian)的(de)改動，都屬于(yu)(yu)本發(fa)(fa)明的(de)構(gou)思和所(suo)附權利要求的(de)保護范(fan)圍。