一種基于直線位移傳感器的工業(yè)機器人運動學(xué)參數(shù)的標(biāo)定算法
【專利摘要】本發(fā)明公開一種基于直線位移傳感器的工業(yè)機器人運動學(xué)參數(shù)的標(biāo)定算法，屬于機器人技術(shù)領(lǐng)域。該算法將傳感器固定于機器人工作空間內(nèi)若干位置，根據(jù)示教程序指令讓機器人運動到不同的指定位置停止，并通過傳感器獲取機器人末端TCP與傳感器基準(zhǔn)位置的距離數(shù)據(jù)，記錄關(guān)節(jié)角度值；然后進行粗標(biāo)定，利用機器人TCP獲得傳感器的基準(zhǔn)位置的初始值，根據(jù)測量數(shù)據(jù)和標(biāo)定方程，獲得傳感器基準(zhǔn)位置的較準(zhǔn)確值，只考慮關(guān)節(jié)角度零位偏差得到標(biāo)定方程，完成粗標(biāo)定；接著進行精標(biāo)定，考慮機器人所有DH參數(shù)、末端連桿參數(shù)、傳感器零位偏差，建立標(biāo)定方程，多次迭代后完成標(biāo)定。本發(fā)明具有結(jié)構(gòu)簡單、容易實現(xiàn)、標(biāo)定精度高及收斂速度快等特點。
【專利說明】
一種基于直線位移傳感器的工業(yè)機器人運動學(xué)參數(shù)的標(biāo)定 算法
技術(shù)領(lǐng)域：
[0001] 本發(fā)明屬于機器人技術(shù)領(lǐng)域，具體涉及一種基于直線位移傳感器的工業(yè)機器人運 動學(xué)參數(shù)的標(biāo)定算法。
【背景技術(shù)】：
[0002] 隨著工業(yè)機器人在工業(yè)生產(chǎn)中應(yīng)用范圍及其任務(wù)復(fù)雜程度的不斷擴大，對工業(yè)機 器人的位置和姿態(tài)精度要求也越來越高。目前工業(yè)機器人具有很高的重復(fù)定位精度，在 0.1mm以內(nèi)，而絕對定位精度卻很低，在1mm以上，甚至達到lcm，這嚴(yán)重限制了工業(yè)機器人的 應(yīng)用范圍。眾多原因?qū)е鹿I(yè)機器人定位精度低，其中最重要的是運動學(xué)模型中幾何結(jié)構(gòu) 的參數(shù)偏差。標(biāo)定技術(shù)是補償這些參數(shù)偏差的有效方法，因此成為研究熱點。標(biāo)定就是應(yīng)用 先進的測量手段和基于模型的參數(shù)識別方法辨識出機器人模型的準(zhǔn)確參數(shù)，從而提高機器 人絕對定位精度。
[0003] 三十余年來，機器人標(biāo)定領(lǐng)域已經(jīng)取得了一些令人矚目的成果。測量方法主要有 兩類方法，其中一類方法需要高精度的測量設(shè)備來精確測量機器人末端的位置向量，比如 三坐標(biāo)測量機和激光跟蹤儀等。這些設(shè)備昂貴，限制了其應(yīng)用場合。另一類方法是在機器人 末端施加一些約束形成運動學(xué)閉鏈，主要是平面約束和球約束。這方法需要復(fù)雜精確的手 工操作和效率不高。隨著高精度直線位移傳感器的發(fā)展，如拉線傳感器，利用拉線傳感器進 行機器人標(biāo)定具有便于攜帶，低成本，操作方便的特點。但是由于直線位移傳感器本身特 點，如精度相對于激光跟蹤儀來說很低，測量數(shù)據(jù)是一維的（只能測量兩點之間的距離)等， 限制了其在機器人標(biāo)定領(lǐng)域的應(yīng)用，因此開發(fā)基于直線位移傳感器的機器人運動學(xué)參數(shù)標(biāo) 定算法成為迫切需求。

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

[0004] 本發(fā)明的目的在于提供一種基于直線位移傳感器的工業(yè)機器人標(biāo)定算法，該算法 標(biāo)定精度高，收斂速度快，并且易于編程實現(xiàn)，能夠標(biāo)定工業(yè)機器人的所有連桿參數(shù)，主要 包括關(guān)節(jié)角度的零位偏差、連桿長度、連桿扭角、連桿偏置，本算法能標(biāo)定拉線傳感器的零 位偏差。
[0005] 直線位移傳感器以拉線傳感器為首選，拉線傳感器具有成本低，精度高，量程大， 體積小等優(yōu)點，十分適合機器人現(xiàn)場標(biāo)定。由于機器人標(biāo)定對傳感器的要求是精度高，量程 大，但拉線傳感器量程越大精度越低，為了保證標(biāo)定精度，選用小量程高精度的拉線傳感 器。為了提高標(biāo)定效果，標(biāo)定點需盡可能遍布機器人全工作空間，這與拉線傳感器小量程形 成矛盾。為了解決這一矛盾，本發(fā)明標(biāo)定過程中，拉線傳感器可固定在若干位置進行測量。 每一位置采集機器人工作空間的部分區(qū)域，這樣可采用小量程拉線傳感器;多個位置采集， 就可覆蓋機器人全工作空間。
[0006] 本發(fā)明所提供的一種基于直線位移傳感器的工業(yè)機器人標(biāo)定算法包括以下步驟：
[0007] 第一步:直線位移傳感器固定在若干位置獲取測量數(shù)據(jù)：
[0008] (1.1)直線位移傳感器固定在某任意位置，對工業(yè)機器人進行示教；
[0009] (1.2)根據(jù)示教程序指令讓所述工業(yè)機器人運動到指定位置停止，并記錄所述工 業(yè)機器人在不同位置時所述直線位移傳感器的數(shù)據(jù)及所述工業(yè)機器人的關(guān)節(jié)角度值；
[001 ο] (1.3)改變所述直線位移傳感器的位置并固定，對所述工業(yè)機器人進行示教，重復(fù) 步驟(1.2);
[0011] (1.4)重復(fù)步驟(1.3)若干次，一般情況下直線位移傳感器位置數(shù)為1到3;
[0012] 第二步:直線位移傳感器固定在每個位置時的粗標(biāo)定：
[0013] (2.1)首先讓所述工業(yè)機器人末端TCP盡可能靠近所述直線位移傳感器，獲取TCP 位置，并把此位置作為所述直線位移傳感器的基準(zhǔn)位置初值；
[0014] (2.2)根據(jù)所述直線位移傳感器的基準(zhǔn)位置初值、測量數(shù)據(jù)和所述機器人名義參 數(shù)，標(biāo)定所述直線位移傳感器的基準(zhǔn)位置；
[0015] (2.3)根據(jù)標(biāo)定出的所述直線位移傳感器基準(zhǔn)位置與建立的標(biāo)定模型對所述工業(yè) 機器人的關(guān)節(jié)零位偏差和所述直線位移傳感器基準(zhǔn)位置偏差進行粗標(biāo)定；
[0016] 第三步:綜合所述直線位移傳感器所有位置粗標(biāo)定結(jié)果的精標(biāo)定：
[0017] (3.1)建立標(biāo)定模型，對所述工業(yè)機器人所有DH參數(shù)、末端連桿參數(shù)和所述直線位 移傳感器的零位偏差進行標(biāo)定；
[0018] (3.2)迭代標(biāo)定，多次重復(fù)步驟(3.1)，一般重復(fù)步驟(3.1)四次后標(biāo)定結(jié)果變化足 夠小，標(biāo)定結(jié)束。
[0019] 本發(fā)明提供的機器人標(biāo)定算法，算法結(jié)構(gòu)簡單，容易實現(xiàn)，并且標(biāo)定精度高，收斂 速度快。采用本發(fā)明的標(biāo)定算法和市場上高精度拉線傳感器，能對機器人的結(jié)構(gòu)參數(shù)進行 校準(zhǔn)，以提高機器人的絕對定位精度。同時可在中小企業(yè)得到廣泛運用，補償機器人結(jié)構(gòu)因 長期工作而造成的損耗，為機器人長時間高可靠性工作提供質(zhì)量保障。涉及用于工業(yè)機器 人在各個行業(yè)的運用，如飛機裝配孔、弧焊、點焊、噴漆等。
【附圖說明】：
[0020] 圖1是基于直線位移傳感器的工業(yè)機器人標(biāo)定算法流程圖；
[0021] 圖2是本發(fā)明所涉及的機器人標(biāo)定系統(tǒng)。
[0022] 圖中：1:機器人控制器1; 2:待標(biāo)定的工業(yè)機器人;3:連接桿;4:拉線傳感器，5:數(shù) 據(jù)采集卡;6:計算機。
【具體實施方式】：
[0023]下面結(jié)合附圖和具體實施例進一步說明本發(fā)明的技術(shù)方案。
[0024]如圖2所示。直線位移傳感器采用拉線傳感器，當(dāng)然也可采用其他傳感器，如激光 跟蹤儀。標(biāo)定系統(tǒng)由機器人控制器1，待標(biāo)定的工業(yè)機器人2,連接桿3,拉線傳感器4,數(shù)據(jù)采 集卡5和計算機6組成。基本過程是:待標(biāo)定的工業(yè)機器人2安裝在某一固定位置上，工業(yè)機 器人的末端法蘭安裝有與拉線傳感器匹配的連接桿3,拉線傳感器4盡量固定放置在工業(yè)機 器人工作空間內(nèi)，在這一原則下，拉線傳感器的位置可以任意，待標(biāo)定的工業(yè)機器人2在機 器人控制器1的驅(qū)動下運動到空間各個不同的位置，裝有標(biāo)定軟件的計算機6通過數(shù)據(jù)采集 卡5獲得測量數(shù)據(jù)。多次改變拉線傳感器4的位置并固定，拉線傳感器4盡量固定放置在工業(yè) 機器人工作空間內(nèi)，在這一原則下，拉線傳感器的位置可以任意，進行測量。根據(jù)所有測量 數(shù)據(jù)最終標(biāo)定出機器人運動學(xué)參數(shù)。
[0025] 對工業(yè)機器人標(biāo)定前，首先對工業(yè)機器人進行示教，示教的目的是使示教點盡量 分散在機器人工作范圍內(nèi)，確保工業(yè)機器人的結(jié)構(gòu)誤差對機器人絕對定位精度影響最大。 需要工業(yè)機器人按照指令運動規(guī)定位置并停止，通過拉線傳感器對每個示教點位置進行測 量。本發(fā)明中拉線傳感器固定在多個位置，在每個位置的示教點盡量分散;多個位置的全部 不教點盡量分散在機器人整個工作范圍內(nèi)。每個位置的不教點一般在100左右。
[0026] 該算法的關(guān)鍵就是將拉線傳感器當(dāng)作工業(yè)機器人誤差傳遞的一部分，并且粗精標(biāo) 定分開。
[0027] 首先進行粗標(biāo)定，目的是獲得拉線傳感器每個位置的精確坐標(biāo)。為了公式簡潔，粗 標(biāo)定時拉線傳感器在每個位置時，不加下標(biāo)區(qū)別，采用的測量數(shù)據(jù)僅限于該位置時的測量 數(shù)據(jù)。拉線傳感器坐標(biāo)在工業(yè)機器人的基座坐標(biāo)系中定義，記為基準(zhǔn)點P Q(XQ，y()，z〇)。
[0028] 在具體的標(biāo)定過程中，需要工業(yè)機器人按照示教好的程序運動到示教位置，記工 業(yè)機器人運動到每個位置的TCP實際坐標(biāo)為Pai ( Xai，yai，Zai )，名義坐標(biāo)為Pni ( Xni，yni，Zni )， 名義坐標(biāo)是根據(jù)機器人前向運動學(xué)，采用機器人名義參數(shù)(設(shè)計參數(shù))計算出的。通過拉線 傳感器對每個示教點位置進行測量，那么通過拉線傳感器測量獲得的就是基準(zhǔn)點P〇到示教 點P al的長度h，該長度由下式定義：
[0029] (XQ-xai)2+(y〇-yai) 2+(z『zai)2 = li2 (1)
[0030] 本發(fā)明提出根據(jù)測量數(shù)據(jù)計算出基準(zhǔn)點PQ的預(yù)估值。
[0031] 下面進行粗標(biāo)定，首先讓工業(yè)機器人TCP盡可能靠近拉線傳感器，獲取此時TCP名 義坐標(biāo)，并把此坐標(biāo)作為拉線傳感器的基準(zhǔn)位置初值Po '（X ' 0，y ' ο，Z ' 0)，則拉線傳感器基準(zhǔn) 位置偏差Δ P〇:
[0032] Δ Ρ〇( Δ X，Δ y，Δ ζ) = Δ P〇(x〇-x，〇，y〇-y，〇，z〇-Z，。） (2)
[0033] 由于工業(yè)機器人還未標(biāo)定，不能得到示教點Pal，故采用Pni近似，帶入方程（1)，得 到：
[0034] (x〇-xni)2+(y〇-yni) 2+(z『zni)2 = li2 (3)
[0035] 任意取兩個測量點i，j的方程進行相減，整理得到： (? - Χ,η ) Λ0 + (J'"/ - )\η ) J'O + ( Z," - Z"i )Z0=1ij ,.
[0036] / ,.. 、r . '、 ⑷ k = ~ ^~J + (? + + ZH) ) ~ (-T<H +)m+ -ni ))^ 2
[0037] 方程(2)帶入方程(4)，整理得到： Γ ? (? - ) ^ + () 4v + (zuj - ζ",.)Δζ = /；;
[0038] Γ/ , , , , , , , ,? (5)
[0039] 方程(5)中只有拉線傳感器基準(zhǔn)位置偏差是未知的，且是位置偏差的線性方程，取 拉線傳感器在該位置的所有測量數(shù)據(jù)的組合，則可得到ef個方程，N是該位置的示教點數(shù)。 通過最小二乘法計算得到位置偏差A(yù) PQ，獲得拉線傳感器基準(zhǔn)位置的標(biāo)定初值PQ '（X ' 〇+Δ 叉，7'〇+"，2'0+六2)，為了方便表示，此標(biāo)定初值還是記為?()'（1'()，7'()，2'0)。
[0040] 由于工業(yè)機器人的絕對定位精度主要受關(guān)節(jié)零位偏差影響，此時只標(biāo)定工業(yè)機器 人的關(guān)節(jié)零位偏差。由于方程(3)采用名義位置Ρηι，故拉線傳感器基準(zhǔn)位置標(biāo)定初值還是存 在較大偏差，需和機器人的關(guān)節(jié)零位偏差參數(shù)同時標(biāo)定。
[0041] 只考慮關(guān)節(jié)零位偏差時，機器人TCP實際位置Pal和名義位置Ρηι之間關(guān)系為：
[0042] Xai - Xni+Jix0 Δ Θ，yai - yni+Jiy0 Δ θ，Zai - Zni+Jiz0 Δ θ (6)
[0043] 其中△ θ是關(guān)節(jié)零位偏差列向量，Jlx0是機器人χ坐標(biāo)方程對關(guān)節(jié)角度的雅可比行 向量，其他兩個雅可比行向量分別對應(yīng)y和z坐標(biāo)，三個雅可比行向量均為已知值。
[0044] 拉線傳感器基準(zhǔn)位置的實際位置與標(biāo)定初值的偏差，如方程(2)定義，為了方便表 示，還是記為Δ Ρ〇( Δ X，Δ y，Δ z)。
[0045] 將方程(6)和拉線傳感器基準(zhǔn)位置偏差，帶入方程（1 )，忽略偏差的二次項，整理得 到： Γ ? (?&4?+ahJ,ve + aiJte)ΑΘ+(?^+aivAy+ ai^) = Vi -(4+ al + al)172
[0046] ,. , ' (7) ah- = χ0 -χ"?：αΗ = Jo = Z0 ~-ni
[0047] 方程(7)中只有角度零位偏差和基準(zhǔn)位置偏差是未知，且是偏差的線性方程，取所 有的N個示教點數(shù)據(jù)，得到N個方程，通過最小二乘法計算得到這些偏差，獲得關(guān)節(jié)角度零位 偏差和拉線傳感器基準(zhǔn)位置的粗標(biāo)定位置，為了方便表示，此粗標(biāo)定位置還是記為Po '（ X ' 〇， y ' ο，ζ ' 〇)。這樣完成了拉線傳感器一個位置的粗標(biāo)定。
[0048] 對拉線傳感器每個位置的測量數(shù)據(jù)進行粗標(biāo)定，得到拉線傳感器每個位置對應(yīng)的 基準(zhǔn)位置的粗標(biāo)定位置。
[0049] 下面進行精標(biāo)定，此時可標(biāo)定工業(yè)機器人所有DH參數(shù)，當(dāng)然也包括工業(yè)機器人末 端連桿參數(shù)，同時拉線傳感器一般存在零位偏差，為了提高標(biāo)定精度，必須對此零位偏差同 時進行標(biāo)定。
[0050] 為了公式簡潔，拉線傳感器在每個位置，推導(dǎo)精標(biāo)定的標(biāo)定模型時，不加下標(biāo)區(qū) 另IJ。最后綜合所有位置的模型時，加下標(biāo)以區(qū)別。
[0051 ]考慮拉線傳感器零位偏差時，測量數(shù)據(jù)表示為：
[0052] lai = li+A 1 (8)
[0053] lai是實際距尚，li是測量距尚，Δ 1是零位偏差。
[0054] 方程（1)變?yōu)椋?br>[0055] (x〇-xai)2+(y〇-yai) 2+(z『zai)2 = lai2 = (li+A I)2 (9)
[0056] 當(dāng)工業(yè)機器人標(biāo)定參數(shù)為Δρ列向量時，機器人TCP實際位置Pal和名義位置Ρηι之 間關(guān)系為：
[0057] Xai - Xni+Jixp Δ p，yai - yni+jiyp Δ p，Zai - Zni+Jizp Δ p (10)
[0058] 其中JixP是工業(yè)機器人x坐標(biāo)方程對標(biāo)定參數(shù)向量ΛΡ的雅可比行向量，其他兩個 雅可比行向量分別對應(yīng)y和ζ坐標(biāo)，三個雅可比行向量均為已知值。
[0059] 拉線傳感器基準(zhǔn)位置的實際位置與標(biāo)定初值的偏差，如方程(2)定義，為了方便表 示，還是記為Δ Ρ〇( Δ X，Δ y，Δ z)。
[0060] 將方程(8)、（10)和拉線傳感器基準(zhǔn)位置偏差，帶入方程(9)，忽略偏差的二次項， 整理得到： 「 n (Ciix^ixp +a/v^Vp +α?ζ^?ζρ)^Ρ^(α?χ^Χ + α?Λ? + α?ι^)~^Α^ ~(α?χ ^aiy +^)]/2 .
[0061] , , ., U1) α?χ ~ Χ0 ~Xm^aiy ~ Το ~ )：η?^α?= ^ ~0 ~2η?
[0062] 方程（11)中只有機器人標(biāo)定參數(shù)向量Δρ、傳感器基準(zhǔn)位置偏差ΔΡ〇(Δχ，Δγ，Δ ζ)和零位偏差△ 1是未知，且是偏差的線性方程。
[0063] 如果對每個拉線傳感器位置的測量數(shù)據(jù)，都分別采用方程（11)，通過最小二乘法 計算得到工業(yè)機器人標(biāo)定參數(shù)。由于拉線傳感器每個位置的標(biāo)定結(jié)果不同，如何融合這些 結(jié)果，是個難題。只是簡單的取平均值，效果不佳。
[0064] 本發(fā)明采用整體標(biāo)定的方法，即利用拉線傳感器所有位置的測量數(shù)據(jù)，建立方程 (11 )，把所有方程整合成一個大的線性方程組，進行標(biāo)定。
[0065] 仔細研究方程（11)，發(fā)現(xiàn)只有傳感器基準(zhǔn)位置偏差Δ Ρ〇( Δ X，Δ y，Δ ζ)與拉線傳 感器的位置有關(guān)，工業(yè)機器人標(biāo)定參數(shù)向量A p、拉線傳感器零位偏差Δ1與拉線傳感器位 置無關(guān)。
[0066] 令k = l，...，m為拉線傳感器位置編號，m為拉線傳感器不同位置數(shù)，一般取1到3， 每個位置的示教點數(shù)為池=沁，...，N m，拉線傳感器每個位置的實際坐標(biāo)為粗 標(biāo)定的坐標(biāo)為<??)，位置偏差為M ΔζΑ)。
[0067] 把拉線傳感器每個位置的測量數(shù)據(jù)帶入方程(11)，得到： ianJnp+anJnp+aiz J,：p)AP + ia,M + = ^ -(an + al + al)V 2
[0068 ] αΙ? = 4' - ·η", % = )i' ~ )Μ· uh = 4' - -7," (12) k = 1,…
[0069] 綜合所有示教點，可得?乂個線性方程，通過最小二乘法解此線性方程組，可得到 工業(yè)機器人標(biāo)定參數(shù)向量△ ρ、拉線傳感器零位偏差△ 1和拉線傳感器在每個位置的基準(zhǔn)位 置偏差^/^^4/，&^。利用這些標(biāo)定偏差對機器人和拉線傳感器進行更新。顯然上述精 標(biāo)定可迭代進行，來提高標(biāo)定精度，一般迭代四次后，標(biāo)定結(jié)果改變很小，標(biāo)定結(jié)束。
[0070] 以上所有的線性方程組的最小二乘法，可以采用普通最小二乘法，但優(yōu)先采用基 于奇異值分解的最小二乘法，以提高解算穩(wěn)定性和精度。
[0071] 以上所述，僅為本發(fā)明最佳實施方式，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明 披露的技術(shù)范圍內(nèi)，可顯而易見地得到的技術(shù)方案的簡單變化或等效替換均落入本發(fā)明的 保護范圍內(nèi)。
【主權(quán)項】
1. 一種基于直線位移傳感器的工業(yè)機器人運動學(xué)參數(shù)的標(biāo)定算法，其特征在于該標(biāo)定 算法包括以下步驟： 第一步:直線位移傳感器固定在若干位置獲取測量數(shù)據(jù)： (1.1) 直線位移傳感器固定在某任意位置，對工業(yè)機器人進行示教； (1.2) 根據(jù)示教程序指令讓所述工業(yè)機器人運動到指定位置停止，并記錄所述工業(yè)機 器人在不同位置時所述直線位移傳感器的數(shù)據(jù)及所述工業(yè)機器人的關(guān)節(jié)角度值； (1.3) 改變所述直線位移傳感器的位置并固定，對所述工業(yè)機器人進行示教，重復(fù)步驟 (1.2)； (1.4) 重復(fù)步驟(1.3)若干次，一般情況下直線位移傳感器位置數(shù)為1到3; 第二步:直線位移傳感器固定在每個位置時的粗標(biāo)定： (2.1) 首先讓所述工業(yè)機器人末端TCP盡可能靠近所述直線位移傳感器，獲取TCP位置， 并把此位置作為所述直線位移傳感器的基準(zhǔn)位置初值； (2.2) 根據(jù)所述直線位移傳感器的基準(zhǔn)位置初值、測量數(shù)據(jù)和所述機器人名義參數(shù)，標(biāo) 定所述直線位移傳感器的基準(zhǔn)位置； (2.3) 根據(jù)標(biāo)定出的所述直線位移傳感器基準(zhǔn)位置與建立的標(biāo)定模型對所述工業(yè)機器 人的關(guān)節(jié)零位偏差和所述直線位移傳感器基準(zhǔn)位置偏差進行粗標(biāo)定； 第三步:綜合所述直線位移傳感器所有位置粗標(biāo)定結(jié)果的精標(biāo)定： (3.1) 建立標(biāo)定模型，對所述工業(yè)機器人所有DH參數(shù)、末端連桿參數(shù)和所述直線位移傳 感器的零位偏差進行標(biāo)定； (3.2) 迭代標(biāo)定，多次重復(fù)步驟（3.1)，一般重復(fù)步驟（3.1)四次后標(biāo)定結(jié)果變化足夠 小，標(biāo)定結(jié)束。2. 根據(jù)權(quán)利要求1所述的標(biāo)定算法，其特征在于所述直線位移傳感器采用拉線傳感器。
【文檔編號】G06T7/20GK106097395SQ201610460753
【公開日】2016年11月9日
【申請日】2016年6月21日
【發(fā)明人】單建華, 佘慧莉
【申請人】安徽工業(yè)大學(xué)