曹 彤,郜 夯,劉 達(dá)
(1.北京科技大學(xué)機(jī)械工程學(xué)院,北京 100089;2.北京航空航天大學(xué)機(jī)器人研究所,北京 100089)
隨著現(xiàn)代機(jī)器人的不斷發(fā)展,各式傳感器在機(jī)器人應(yīng)用領(lǐng)域扮演著越來越重要的角色。在現(xiàn)在較為流行的協(xié)作機(jī)器人和傳統(tǒng)的工業(yè)機(jī)器人[1]的工作中,六維力/矩傳感器大放異彩,起到了不可替代的作用。它大大提高了機(jī)器人的感知能力,帶來了良好的人機(jī)交互體驗(yàn)[2]。六維力/矩傳感器通常被安裝在機(jī)器人末端與所用工具之間,用以實(shí)時(shí)測量工具所受的外力。但是由于工具重力、傳感器自身重力,以及傳感器初值的存在,當(dāng)未施加外力的時(shí)候傳感器上會(huì)有所顯示,造成示數(shù)的偏差。其中傳感器的零位初值是一個(gè)受多方面因素影響的參數(shù),末端工具和傳感器之間的不同連接方式以及連接強(qiáng)度會(huì)對傳感器的零位初值造成影響[3],稱之為安裝誤差。它的存在使得傳感器在未加載時(shí)不能準(zhǔn)確置零。安裝誤差無法在空載的時(shí)候通過相關(guān)方法計(jì)算或者補(bǔ)償,故必須在負(fù)載安裝完畢的條件下進(jìn)行。因此,有必要對機(jī)器人示教過程中的六維力傳感器的安裝誤差進(jìn)行精確計(jì)算。
為了降低安裝誤差帶來的影響,國內(nèi)外學(xué)者對于傳感器的初值的計(jì)算也進(jìn)行過相關(guān)的推導(dǎo)與實(shí)驗(yàn),并且取得了一定的實(shí)際效果。文獻(xiàn)[4]提出了一個(gè)計(jì)算傳感器初值的基本公式,對后人的影響很大;文獻(xiàn)[5]提出了一種通過六個(gè)特殊位置的相關(guān)信息直接計(jì)算得到傳感器零位初值的方法;文獻(xiàn)[3]通過獲取大量傳感器數(shù)據(jù)后利用最小二乘法等來計(jì)算傳感器初值以及機(jī)器人的安裝傾角。還有很多學(xué)者對這個(gè)問題根據(jù)不同的實(shí)際情況,進(jìn)行了深入的研究與實(shí)驗(yàn),很有借鑒價(jià)值。但是從文獻(xiàn)中可以發(fā)現(xiàn),很多研究過程中沒有考慮到這個(gè)負(fù)載安裝引起的誤差,或者將其歸到出廠前廠商進(jìn)行校準(zhǔn)和補(bǔ)償?shù)姆秶?。還有一些將傳感器自身重力對示數(shù)的影響也忽略了,這對于機(jī)器人進(jìn)行精密操作可能會(huì)有嚴(yán)重的影響。
針對很多研究將安裝誤差歸為出廠前調(diào)試補(bǔ)償而不進(jìn)行相關(guān)計(jì)算等現(xiàn)狀,提出了一種簡單又有效的安裝誤差的計(jì)算方法來幫助傳感器置零。利用變換矩陣在不同坐標(biāo)系的變換以補(bǔ)償工具重力以及傳感器自身重力,結(jié)合傳感器實(shí)時(shí)的數(shù)值計(jì)算得到傳感器安裝誤差,以便于傳感器的置零操作。
對于一個(gè)六自由度的協(xié)作機(jī)器人來說,傳感器安裝在機(jī)器人末端與工具之間,用于獲取末端工具受力情況。為了計(jì)算出工具重力、傳感器自身重力影響以及傳感器初值,需要以機(jī)器人本體、六維力/矩傳感器、末端工具為研究對象,研究和推導(dǎo)傳感器初值的計(jì)算方法。以Universal-Robots公司的UR5型機(jī)器人和ATI公司的FT21729型六維力傳感器為研究對象,UR5型機(jī)器人是一個(gè)六自由度的串聯(lián)協(xié)作機(jī)器人,共有六個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié),較為符合實(shí)驗(yàn)要求,如圖1所示。FT21729型六維力傳感器是一個(gè)具有高精度、高頻率特點(diǎn)的傳感器,如圖2所示。
圖1 UR5機(jī)器人Fig.1 UR5 Robot
圖2 ATI六維力傳感器Fig.2 ATI Net Force/Torque Sensor
根據(jù)D-H法對機(jī)器人建立坐標(biāo)系[6],如圖3所示。機(jī)器人基座設(shè)為0系{x0,y0,z0},機(jī)器人末端為6系{x6,y6,z6}。定義各桿的參數(shù),列出機(jī)器人坐標(biāo)系對應(yīng)的D-H參數(shù)表,如表1所示。機(jī)器人末端的傳感器和工具的連接順序,如圖4所示。針對這種連接方式,機(jī)器人末端的傳感器系和工具系的建立,如圖5所示。其中,傳感器系記為S{Sx,Sy,Sz},工具系記為T{Tx,Ty,Tz}。傳感器系和工具系的坐標(biāo)原點(diǎn)分別建立在傳感器和工具的重心位置,由于傳感器質(zhì)心的輕微偏移以及末端工具的不同形狀,它們的重心會(huì)分布在機(jī)器人末端6系的z軸延長線的周圍。末端工具重力大小及重心位置的求取可參考文獻(xiàn)[7-8]等,在此不予討論。
圖3 機(jī)器人D-H坐標(biāo)系Fig.3 Robot D-H Coordinate System
表1 機(jī)器人D-H參數(shù)表Tab.1 Robot D-H Parameter Table
圖4 傳感器和末端工具的連接方式Fig.4 Connection Mode of Sensor and End Tool
圖5 傳感器系和工具系的建立Fig.5 Establishment of Sensor System and Tool System
前置坐標(biāo)系的變換矩陣為:
將表1數(shù)據(jù)帶入公式中,可以依次獲得從機(jī)器人基系到機(jī)器人末端6系的位姿變換矩陣T01(T01表示從基系到1系的坐標(biāo)轉(zhuǎn)化,以此類推)、T12、T23、T34、T45、T56。按照次序?qū)⒏鱾€(gè)轉(zhuǎn)換矩陣相乘可以得到0系到6系的坐標(biāo)轉(zhuǎn)換矩陣T06:
根據(jù)傳感器重心相對于傳感器末端的位置,可以得到6系到傳感器系的轉(zhuǎn)換矩陣:
式中:Sx—傳感器重心相對于機(jī)器人末端6系原點(diǎn)x方向上的偏移量;Sy—傳感器重心相對于機(jī)器人末端6系原點(diǎn)y方向上的偏移量;Sz—傳感器重心相對于機(jī)器人末端6系原點(diǎn)z方向上的偏移量。
在計(jì)算工具坐標(biāo)系T相對于傳感器坐標(biāo)系S的轉(zhuǎn)換矩陣時(shí),由于轉(zhuǎn)換矩陣的第四列是工具重心相對于傳感器重心的位置,故轉(zhuǎn)換矩陣中的p列為:
式中:Tx—工具重心相對于傳感器末端中心x方向上的偏移量;Ty—工具重心相對于傳感器末端中心y方向上的偏移量;Tz—工具重心相對于傳感器末端中心z方向上的偏移量。
則傳感器坐標(biāo)系S到工具坐標(biāo)系T的轉(zhuǎn)換矩陣TST為:
根據(jù)以上轉(zhuǎn)換矩陣可以得到從機(jī)器人基系到工具重心的坐標(biāo)轉(zhuǎn)換矩陣T0T為:
至此得到所需全部坐標(biāo)轉(zhuǎn)換矩陣。
工具重力在基系坐標(biāo)系下的大小為G,方向豎直向下,而它的作用點(diǎn)在工具系的原點(diǎn)。所以想要得到工具重力在工具系下的大小,需要將其轉(zhuǎn)換到工具系中。根據(jù)文獻(xiàn)[9],兩個(gè)坐標(biāo)系之間廣義操作力的坐標(biāo)變換關(guān)系為:
由于工具的重力只產(chǎn)生了力的作用效果,沒有產(chǎn)生力矩,故基系上的工具重力可以表示為[10]:
將相關(guān)數(shù)據(jù)帶入式(7),可以將基系上的工具重力轉(zhuǎn)化到工具坐標(biāo)系,得到工具坐標(biāo)系上的工具重力(T fTn)T為:
這樣就得到了工具的重力在工具坐標(biāo)系上的數(shù)值。因傳感器測得的數(shù)值是外力在傳感器系上作用的體現(xiàn),故當(dāng)對工具重力進(jìn)行補(bǔ)償時(shí),需要將這個(gè)重力轉(zhuǎn)化到傳感器系上進(jìn)行操作。利用公式:
可以進(jìn)一步得到工具的六維力在傳感器系上的數(shù)值大小,即工具重力引起的傳感器數(shù)值變化,這就是所需要補(bǔ)償?shù)墓ぞ咧亓χ怠?/p>
在查閱文獻(xiàn)的過程中,發(fā)現(xiàn)在一些研究過程中將傳感器自身重力引起的示數(shù)變化也歸結(jié)到廠商進(jìn)行補(bǔ)償?shù)姆秶?,只進(jìn)行了工具重力補(bǔ)償后便直接使用六維力/矩傳感器。六維傳感器是由上平臺(tái)和下平臺(tái)以及其他的一些部件構(gòu)成,工作的主要原理是通過外力作用使得上下平臺(tái)之間產(chǎn)生微小位移,再將該微小位移通過相關(guān)算法轉(zhuǎn)化為相應(yīng)的力和力矩。在實(shí)際應(yīng)用時(shí)會(huì)發(fā)現(xiàn),僅進(jìn)行工具重力補(bǔ)償后的傳感器示數(shù)同樣會(huì)隨著傳感器位置的不同而出現(xiàn)明顯的變化。經(jīng)分析可知是由傳感器自身的重力引起上下平臺(tái)的位移而產(chǎn)生的示數(shù)變化,故需要對其進(jìn)行補(bǔ)償。
根據(jù)圖4建立的機(jī)器人末端坐標(biāo)系和傳感器坐標(biāo)系,可以推知從機(jī)器人基系到傳感器坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣T0S:
在基系中,傳感器自身重力的大小為Gs,方向豎直向下。要想獲得該重力在傳感器系中的六維力的大小,同樣需要使用兩個(gè)坐標(biāo)系之間的廣義操作力坐標(biāo)變換式(7),在這里具體表現(xiàn)為:
通過這個(gè)公式可以將傳感器自身重力從基系轉(zhuǎn)換到傳感器系上,即傳感器自身重力引起的傳感器數(shù)值的變換,這就是需要進(jìn)行補(bǔ)償?shù)膫鞲衅髦亓χ怠?/p>
前面兩部分講到由于重力引起的傳感器示數(shù)的變化,它們需要進(jìn)行實(shí)時(shí)補(bǔ)償。傳感器在制造過程中,由于原理、工藝等多方面的因素,會(huì)使得傳感器在未安裝負(fù)載時(shí)就有示數(shù),在出廠前廠商會(huì)對這個(gè)初值進(jìn)行校準(zhǔn)和補(bǔ)償。而安裝誤差是在安裝傳感器,以及在傳感器后安裝負(fù)載后引起的傳感器示數(shù)的變化,這一值與連接配件的結(jié)構(gòu)、安裝的緊固方式、安裝的緊固強(qiáng)度等因素有很大的關(guān)系。正是這樣的特點(diǎn)決定了它無法在未安裝傳感器以及末端工具時(shí)得到,也無法通過相關(guān)的工具與技術(shù)直接測量,只能通過間接的計(jì)算來得到。安裝誤差可能對機(jī)器人進(jìn)行的精密操作造成重大影響,不能忽略。
根據(jù)上述的分析,一個(gè)出廠并投入使用的六維力/矩傳感器的示數(shù)主要由以下幾部分組成:傳感器的安裝誤差、工具重力、傳感器自身重力、所受外力。工具重力和傳感器自身重力可以通過計(jì)算來獲得;所受外力無法直接獲取大小和方向,但是可以根據(jù)控制變量法相關(guān)原理將其置零,以減少計(jì)算安裝誤差時(shí)的因素。故可以得到傳感器示數(shù)的組成公式:
式中:Ds—傳感器所顯示的示數(shù)(DisplɑySensor)。
DTG()
θ—工具重力引起的傳感器示數(shù)(DisplɑyToolsG)。
DSG()θ—傳感器自身重力引起的傳感器示數(shù)(DisplɑySensorG)。
DI—傳感器安裝誤差(DisplɑyInstɑllɑtion)。
DO(t)—所受外力(DisplɑyOutside)。
根據(jù)該公式可知,如想得到傳感器的安裝誤差DI,則需要在同一時(shí)刻獲得其余的幾個(gè)量進(jìn)行相關(guān)的計(jì)算。在實(shí)際使用過程中,利用機(jī)器人的位置信息根據(jù)2.2節(jié)和2.3節(jié)計(jì)算得到工具重力和傳感器自身重力的補(bǔ)償數(shù)值,并將所受外力置零使得DO=0。由示數(shù)的組成公式可以推知傳感器安裝誤差DI為:
通過這樣的方法計(jì)算得到的傳感器安裝誤差DI是考慮了負(fù)載安裝等因素后得到的一個(gè)綜合性的計(jì)算結(jié)果。這個(gè)誤差在每次傳感器及工具安裝完成后是一個(gè)定值,故只需要計(jì)算一次,便可在后續(xù)傳感器使用過程中作為固定補(bǔ)償值來使用。所以工具所受外力的值DO(t)為:
通過式(17)可以看到,通過對工具重力、傳感器重力的補(bǔ)償以及傳感器安裝誤差的計(jì)算,可以準(zhǔn)確得到機(jī)器人末端工具所受到的外力和力矩。將這個(gè)力信息及時(shí)反饋到控制系統(tǒng),可以進(jìn)一步實(shí)現(xiàn)機(jī)器人的力覺拖動(dòng)示教控制功能。
為了實(shí)際檢測上述方法的實(shí)際使用效果,利用現(xiàn)有的UR5機(jī)器人和六維力/矩傳感器進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)平臺(tái),如圖6所示。實(shí)驗(yàn)過程設(shè)計(jì)為:將機(jī)器人末端工具指向不同的方向進(jìn)行傳感器的置零操作,把傳感器經(jīng)補(bǔ)償后的示數(shù)作為效果判斷的標(biāo)準(zhǔn),即經(jīng)過補(bǔ)償后得到的傳感器示數(shù)越接近零效果越好。為了在有限的實(shí)驗(yàn)次數(shù)中盡可能多的測試末端工具不同方向的效果,將實(shí)驗(yàn)劃分為五組,分別為末端工具指向右、向上、向后、向下、向左,運(yùn)動(dòng)方向,如圖7所示。與圖6所示機(jī)器人對應(yīng)。在給定的五組大致方向范圍內(nèi),將工具隨機(jī)拖動(dòng)二十次到不同的位置并進(jìn)行傳感器的置零操作,記錄傳感器未加載時(shí)的示數(shù)。最后將得到的數(shù)據(jù)進(jìn)行分析,計(jì)算各組示數(shù)的平均值以及標(biāo)準(zhǔn)差。
圖6 實(shí)驗(yàn)平臺(tái):UR5機(jī)器人和六維力/矩傳感器Fig.6 Experimental Platform:UR5 Robot and Sensor
圖7 實(shí)驗(yàn)分組的方向Fig.7 Direction of Experimental Grouping
為了進(jìn)行有效的對比,將僅進(jìn)行工具重力補(bǔ)償?shù)姆椒ê推渌墨I(xiàn)中的一種比較流行的方法同時(shí)進(jìn)行數(shù)據(jù)計(jì)算,并采集計(jì)算結(jié)果。將這里方法記為方法一,僅進(jìn)行工具重力補(bǔ)償?shù)姆椒ㄓ洖榉椒ǘ?,其他文獻(xiàn)里的方法記為方法三,實(shí)驗(yàn)結(jié)果如下所示。
當(dāng)工具的方向設(shè)為向右時(shí),得到二十組數(shù)據(jù)平均值,如表2所示。從表2中可以看到,方法三在這個(gè)方向上誤差較大,并沒有起到置零的作用,而方法一得到的合力誤差接近于零值,相較方法二降低了3.6N。力矩方面,方法三比方法二降低0.33Nm,方法一較方法二降低了0.42Nm,因此方法一的誤差比方法三的誤差降低21%,具有比較明顯的效果。
表2 工具向右數(shù)據(jù)Tab.2 Data of the Tool in the Right Direction
將該方向的分力和分力矩,如圖8所示。可以看到,方法二具有較大的波動(dòng)性;方法三在個(gè)別方向上的補(bǔ)償效果較好,但整體不穩(wěn)定;方法一補(bǔ)償后各個(gè)方向的分力和力矩均比較穩(wěn)定。
圖8 補(bǔ)償后的均值(工具向右)Fig.8 Mean Value(right)
該方向的標(biāo)準(zhǔn)差,如圖9所示??梢钥吹皆谶@個(gè)方向上方法二對力和力矩有所補(bǔ)償,但穩(wěn)定性不夠理想。方法三對力矩的控制有明顯的效果,但是在該方向上對力的補(bǔ)償穩(wěn)定性不夠好。只有方法一對力和力矩的補(bǔ)償均較為穩(wěn)定。當(dāng)工具的方向設(shè)為向上時(shí),得到的幾何平均值,如表3所示。方法三還是沒能在這個(gè)方向上起到補(bǔ)償作用,而方法一的合力比方法二降低將近4N。力矩方面,方法三比方法二減低了0.39Nm,方法一較方法二降低了0.44Nm,因此方法一的誤差比方法三的誤差降低將近11%。此方向上的分力和分力矩圖,如圖10所示。方法二在力矩的控制上較差,方向間的差異較大;方法三力矩控制的較為理想,但力有個(gè)別方向不受控制,穩(wěn)定性較差。只有方法一較理想,能基本維持在零值附近。該方向的標(biāo)準(zhǔn)差,如圖11所示??梢钥吹椒椒ǘ土氐目刂菩Ч猩仙臻g;方法三的力矩控制穩(wěn)定性較好,但力的控制沒能達(dá)到使用要求。方法一對于力和力矩的補(bǔ)償?shù)姆€(wěn)定性較高。
圖9 力和力矩的標(biāo)準(zhǔn)差(工具向右)Fig.9 Standard Deviation(right)
表3 工具向上數(shù)據(jù)Tab.3 Data of the Tool in the up Direction
圖10 補(bǔ)償后的均值(工具向上)Fig.10 Mean Value(up)
圖11 力和力矩的標(biāo)準(zhǔn)差(工具向上)Fig.11 Standard Deviation(up)
將工具的方向設(shè)為向后,經(jīng)過二十組的實(shí)驗(yàn)計(jì)算后得到幾何平均值,如表4所示。在力的幾何平均值上,方法三較方法二降低6.2N,方法一比方法二降低了9.4N,綜上方法一比方法三誤差降低了63%,精度得到較高提升。力矩方面,相較于方法二,方法三降低了0.23Nm,方法一降低了0.35Nm,方法一比方法三的誤差降低32%。在這個(gè)方向上,方法一同樣具有良好的效果。該方向上的力與力矩的分量均值,如圖12所示。在該方向上,方法二有較大的波動(dòng),沒能達(dá)到補(bǔ)償?shù)男Ч环椒ㄈ齼?yōu)于方法二,但效果遠(yuǎn)遠(yuǎn)不及方法一。方法一能保持各個(gè)方向穩(wěn)定的補(bǔ)償效果。而力和力矩的標(biāo)準(zhǔn)差,如圖13所示。方法三在該方向上可以對力和力矩起到一定的控制作用,但穩(wěn)定性不如方法一,后者在力和力矩的控制上都有較為滿意的效果。當(dāng)把工具的方向設(shè)為向下,方法一不出所料的展示出良好的置零效果,如表5所示。方法二在這個(gè)方向上幾乎沒有起到補(bǔ)償?shù)淖饔?,方法三較方法二減少了1.22N,而方法一幾乎補(bǔ)償了所有的誤差量,比方法二減少了11.55N,和方法三相比誤差降低了89%。力矩方面,相較于方法二,方法三減少了0.41Nm,方法一減少了0.63Nm,誤差降低了34%。
表4 工具向后數(shù)據(jù)Tab.4 Data of the Tool in the Backward Direction
圖12 補(bǔ)償后的均值(工具向后)Fig.12 Mean Value(backward)
圖13 力和力矩的標(biāo)準(zhǔn)差(工具向后)Fig.13 Standard Deviation(backward)
表5 工具向下數(shù)據(jù)Tab.5 Data of the Tool in the Down Direction
查看力和力矩的分量均值,如圖14所示??梢钥吹椒椒ǘ休^大的波紋,幾乎失控;方法三穩(wěn)定性提升很多;方法一依然保持較高的補(bǔ)償水準(zhǔn)。向下方向的標(biāo)準(zhǔn)差,如圖15所示。從中可以看出在該方向上的方法三力的穩(wěn)定性有待提高,方法一在力和力矩兩個(gè)方面依然能保持較高的穩(wěn)定性。工具方向向左的幾何平均值,如表6所示。在力方面,方法三比方法二減少了2.96N,而方法一比方法二減少了8.64N,誤差減少65%。而在力矩方面,方法三降低了0.42Nm,方法一降低了0.47Nm,誤差減少了15.5%。該方向的力和力矩的分量均值,如圖16所示。可以看到,方法二的結(jié)果波動(dòng)極大,各個(gè)方向都有較大誤差;方法三曲線較為平緩,但是遠(yuǎn)遠(yuǎn)不及方法一在各個(gè)方向上的結(jié)果穩(wěn)定。
圖16 補(bǔ)償后的均值(工具向左)Fig.16 Mean Value(left)
表6 工具向左數(shù)據(jù)Tab.6 Data of the Tool in the Left Direction
圖14 補(bǔ)償后的均值(工具向下)Fig.14 Mean Value(down)
圖15 力和力矩的標(biāo)準(zhǔn)差(工具向下)Fig.15 Standard Deviation(down)
該方向上的力與力矩標(biāo)準(zhǔn)差,如圖17所示。從中可以看到,與前幾個(gè)方向的情況相似,方法一繼續(xù)保持著較高的穩(wěn)定性,方法二的穩(wěn)定性還是具有較大的波動(dòng),而方法三介于二者之間。
圖17 力和力矩的標(biāo)準(zhǔn)差(工具向左)Fig.17 Standard Deviation(left)
根據(jù)五組數(shù)據(jù)的均值和標(biāo)準(zhǔn)差比較,方法二在只進(jìn)行工具重力的補(bǔ)償后,不受外力時(shí)傳感器還存在著明顯的示數(shù),直接使用可能會(huì)帶來較大的偏差;方法三在一定程度上可以對傳感器的安裝誤差進(jìn)行補(bǔ)償,且補(bǔ)償力矩的效果優(yōu)于補(bǔ)償力,但補(bǔ)償程度受工具位置的影響較大。而方法一可以有效地幫助傳感器置零,在計(jì)算傳感器安裝誤差時(shí)效果顯著。與現(xiàn)在廣泛使用的一些方法相比,可在力的置零過程中平均減少將近70%的安裝誤差,力矩的置零過程中平均減少22%的安裝誤差。且方法一穩(wěn)定性較高,不受工具方向影響。
通過對機(jī)器人、六維力/矩傳感器以及其末端工具相關(guān)數(shù)據(jù)的一系列計(jì)算,可以推導(dǎo)得到一種有效計(jì)算由負(fù)載安裝引起的傳感器安裝誤差的方法。在進(jìn)行實(shí)驗(yàn)后,得到了多組隨機(jī)位置的傳感器的置零結(jié)果,繪制成表格與圖像進(jìn)行比較。通過比較不同方法間的傳感器的置零效果,可以看出在不需要其他設(shè)備介入的前提下,這里提出的方法具有簡便、效果顯著、穩(wěn)定性高的優(yōu)點(diǎn)。實(shí)驗(yàn)證實(shí)了該方法的正確性與可行性,可以幫助六維力/矩傳感器置零,在實(shí)際使用中獲得更為準(zhǔn)確的所受外力數(shù)值。