本發(fā)明涉及一種工業(yè)機(jī)器人末端工具標(biāo)定技術(shù)，具體涉及一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定方法及裝置。

背景技術(shù)：

隨著工業(yè)機(jī)器人技術(shù)的快速發(fā)展，其逐漸在焊接、切削、裝配等領(lǐng)域得到越來(lái)越廣泛的應(yīng)用。通過(guò)安裝不同的末端工具，工業(yè)機(jī)器人能夠完成多種作業(yè)任務(wù)。其中，工具中心點(diǎn)(Tool Center Point，TCP)的位姿標(biāo)定精度直接影響工業(yè)機(jī)器人的作業(yè)精度。

工業(yè)機(jī)器人的末端工具(如螺柱焊槍、切削刀具等)經(jīng)過(guò)長(zhǎng)時(shí)間工作之后，會(huì)出現(xiàn)一定的位姿偏差，從而導(dǎo)致工業(yè)機(jī)器人無(wú)法完成預(yù)設(shè)功能等問(wèn)題。目前，針對(duì)工業(yè)機(jī)器人末端工具TCP的標(biāo)定主要采用離線(xiàn)人工示教的多點(diǎn)法。該方法首先將工業(yè)機(jī)器人末端工具的TCP對(duì)齊到一固定點(diǎn)上，然后通過(guò)調(diào)整工業(yè)機(jī)器人的關(guān)節(jié)角度，實(shí)現(xiàn)TCP標(biāo)定。但此類(lèi)方法存在一定的弊端：(1)多點(diǎn)法的TCP標(biāo)定過(guò)程主要受人為因素(如操作經(jīng)驗(yàn)等)的影響，不可避免地引入較大的誤差；(2)標(biāo)定過(guò)程耗時(shí)較長(zhǎng)，將影響工業(yè)機(jī)器人的生產(chǎn)作業(yè)效率；(3)為實(shí)現(xiàn)對(duì)生產(chǎn)線(xiàn)地周期性標(biāo)定，將耗費(fèi)大量的人力資源，同時(shí)降低了生產(chǎn)線(xiàn)的總體產(chǎn)能。工業(yè)機(jī)器人TCP快速在線(xiàn)準(zhǔn)確的標(biāo)定直接關(guān)系到生產(chǎn)線(xiàn)的產(chǎn)品質(zhì)量和生產(chǎn)效率，因此，亟待提出一種應(yīng)用于工業(yè)機(jī)器人的TCP快速在線(xiàn)標(biāo)定方法，既能夠保證工業(yè)機(jī)器人的作業(yè)精度，又可以提高工業(yè)機(jī)器人的自動(dòng)化程度。

技術(shù)實(shí)現(xiàn)要素：

為解決現(xiàn)有技術(shù)的不足，本發(fā)明的目的在于提供一種能夠?qū)崿F(xiàn)工業(yè)機(jī)器人 TCP的自由度標(biāo)定的應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定方法及裝置。

為了實(shí)現(xiàn)上述目標(biāo)，本發(fā)明采用如下的技術(shù)方案：

一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置，包括控制柜，TCP標(biāo)定裝置，工業(yè)機(jī)器人、末端工具和控制總線(xiàn)，所述控制柜通過(guò)控制總線(xiàn)分別連接TCP標(biāo)定裝置和工業(yè)機(jī)器人，末端工具安裝在工業(yè)機(jī)器人上；所述TCP標(biāo)定裝置包括TCP檢測(cè)裝置、標(biāo)定控制器和安裝底座，TCP檢測(cè)裝置通過(guò)安裝底座固定安裝在工業(yè)機(jī)器人的一側(cè)，測(cè)量平面與工業(yè)機(jī)器人基坐標(biāo)系的XOY平面平行。

上述TCP檢測(cè)裝置包括裝置上蓋、裝置本體、精度檢驗(yàn)開(kāi)關(guān)以及四組對(duì)射式光電傳感器，所述精度檢驗(yàn)開(kāi)關(guān)設(shè)置于裝置本體上表面，所述裝置本體為縱向鏤空正方形的長(zhǎng)方體，四組對(duì)射式光電傳感器分別設(shè)置于裝置本體內(nèi)側(cè)面的縱向中線(xiàn)上，對(duì)射式光電傳感器的激光射線(xiàn)相互垂直且在同一水平面。

上述標(biāo)定控制器包括裝置外殼、微控制器單元、顯示單元、按鍵單元、狀態(tài)指示燈和通信接口，所述標(biāo)定控制器獲取精度檢驗(yàn)開(kāi)關(guān)的輸出信號(hào)以及四組對(duì)射式光電傳感器的通斷信號(hào)，通過(guò)連接通信接口的控制總線(xiàn)與控制柜進(jìn)行數(shù)據(jù)通信，并通過(guò)顯示單元、狀態(tài)指示燈反饋工作信息。

一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定方法，包括如下步驟：

S1、保持工業(yè)機(jī)器人末端工具垂直于基坐標(biāo)系的XOY平面，通過(guò)人工示教方法實(shí)現(xiàn)工業(yè)機(jī)器人末端工具與TCP檢測(cè)裝置的精度檢測(cè)開(kāi)關(guān)對(duì)準(zhǔn)，記錄工業(yè)機(jī)器人法蘭的空間位姿數(shù)據(jù)；

S2、工業(yè)機(jī)器人連續(xù)工作一段時(shí)間后，工業(yè)機(jī)器人根據(jù)初始人工示教的空間位姿數(shù)據(jù)檢測(cè)當(dāng)前工業(yè)機(jī)器人的TCP點(diǎn)是否偏移較大，若無(wú)法滿(mǎn)足生產(chǎn)要求，則進(jìn)入標(biāo)定程序；

S3、標(biāo)定程序：標(biāo)定控制器通過(guò)控制柜控制工業(yè)機(jī)器人的末端工具以初始 姿態(tài)在TCP檢測(cè)裝置中做正方形軌跡的標(biāo)定運(yùn)動(dòng)；在運(yùn)動(dòng)過(guò)程中，標(biāo)定控制器的微控制器單元監(jiān)測(cè)TCP檢測(cè)裝置內(nèi)的四組對(duì)射式光電傳感器通斷狀態(tài)，根據(jù)該通斷信號(hào)的時(shí)間點(diǎn)，讀取并存儲(chǔ)工業(yè)機(jī)器人在該時(shí)間點(diǎn)的法蘭空間位姿數(shù)據(jù)，利用以上存儲(chǔ)的空間位姿數(shù)據(jù)計(jì)算工業(yè)機(jī)器人末端工具在X軸與Y軸方向上的位置偏差以及角度偏差，并實(shí)現(xiàn)TCP的誤差補(bǔ)償；

S4、重復(fù)步驟S3的操作，通過(guò)標(biāo)定結(jié)果的迭代，降低TCP標(biāo)定誤差；

S5、控制末端工具在基坐標(biāo)系的Z軸方向上做勻速直線(xiàn)運(yùn)動(dòng)，以確定工業(yè)機(jī)器人TCP在Z軸方向上位置偏差，補(bǔ)償TCP的誤差；

S6、重復(fù)步驟S5的操作，通過(guò)標(biāo)定結(jié)果的迭代，降低TCP標(biāo)定誤差。

上述的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定方法，其特征在于所述步驟S3中末端工具在X軸與Y軸方向上的位置偏差的計(jì)算方法為：

在標(biāo)定運(yùn)動(dòng)過(guò)程中，末端工具每經(jīng)過(guò)一組對(duì)射式光電傳感器，標(biāo)定控制器讀取工業(yè)機(jī)器人法蘭坐標(biāo)系的空間位姿數(shù)據(jù)，記為P_ijn；

所述i表示第i組對(duì)射式光電傳感器，i＝1或2或3或4，其中1為對(duì)射式光電傳感器1，2為對(duì)射式光電傳感器2，3為對(duì)射式光電傳感器3，4為對(duì)射式光電傳感器4；

所述j表示對(duì)射式光電傳感器的通斷，j＝1或2，1表示對(duì)射式光電傳感器信號(hào)接通，2表示對(duì)射式光電傳感器信號(hào)斷開(kāi)；

所述n表示單次循環(huán)運(yùn)動(dòng)中，第n次通過(guò)第i組對(duì)射式光電傳感器，n＝1或2；

基于標(biāo)定控制器獲取的法蘭坐標(biāo)系空間位姿數(shù)據(jù)，

A、末端工具第一次通過(guò)第1組對(duì)射式光電傳感器1時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器1的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{11}=\frac{P_{111}.Y+P_{121}.Y}{2},$

末端工具第一次通過(guò)第3組對(duì)射式光電傳感器3時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器3的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{31}=\frac{P_{311}.Y+P_{321}.Y}{2},$

末端工具第二次通過(guò)第1組對(duì)射式光電傳感器1時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器1的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{12}=\frac{P_{112}.Y+P_{122}.Y}{2},$

末端工具第二次通過(guò)第3組對(duì)射式光電傳感器3時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器3的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{32}=\frac{P_{312}.Y+P_{322}.Y}{2},$

因此，末端工具的TCP在Y軸方向上的位置偏差可由下式計(jì)算得出：

${\mathrm{TCP}}_{\mathrm{\Δ}Y}=\frac{Y_{11}-Y_{12}+Y_{31}-Y_{32}}{4};$

B、末端工具第一次通過(guò)第2組對(duì)射式光電傳感器2時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器2的激光射線(xiàn)的交點(diǎn)X軸坐標(biāo)為：

$X_{21}=\frac{P_{211}.X+P_{221}.X}{2},$

末端工具第一次通過(guò)第3組對(duì)射式光電傳感器4時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器4的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$X_{41}=\frac{P_{411}.X+P_{421}.X}{2},$

末端工具第二次通過(guò)第2組對(duì)射式光電傳感器2時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器2的激光射線(xiàn)的交點(diǎn)X軸坐標(biāo)為：

$X_{22}=\frac{P_{212}.X+P_{222}.X}{2},$

末端工具第二次通過(guò)第3組對(duì)射式光電傳感器4時(shí)，末端工具的中線(xiàn)與對(duì)射式光電傳感器4的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$X_{42}=\frac{P_{412}.X+P_{422}.X}{2},$

因此，計(jì)算得到末端工具的TCP在X軸方向上的位置偏差：

${\mathrm{TCP}}_{\mathrm{\Δ}X}=\frac{X_{21}-X_{22}+X_{41}-X_{42}}{4}.$

上述步驟S3中角度偏差的計(jì)算方法為：

上下兩層對(duì)射式光電傳感器1和3之間的距離為d，計(jì)算末端工具的Y方向的角度偏差如下式所示：

${\mathrm{TCP}}_{\mathrm{\Δ}RY}=\arctan \frac{L}{d}=\arctan \left(\frac{X_{41}-X_{21}+X_{42}-X_{22}}{2d}\right),$

上下兩層對(duì)射式光電傳感器2和4之間的距離為d，計(jì)算末端工具的X方向的角度偏差如下式所示：

${\mathrm{TCP}}_{\mathrm{\Δ}RX}=\arctan \frac{L}{d}=\arctan \left(\frac{Y_{31}-Y_{11}+Y_{32}-Y_{12}}{2d}\right).$

上述步驟S5中工業(yè)機(jī)器人TCP在Z軸方向上位置偏差的計(jì)算方法為：

標(biāo)定控制器通過(guò)控制柜控制工業(yè)機(jī)器人的末端工具運(yùn)動(dòng)到對(duì)射式光電傳感器1和2交點(diǎn)的正上方，沿著Z軸方向勻速直線(xiàn)向下運(yùn)動(dòng)，對(duì)射式光電傳感器1和2檢測(cè)到末端工具到達(dá)兩射線(xiàn)交點(diǎn)時(shí)的位姿數(shù)據(jù)，與通過(guò)人工示教得到的初始值P_TCP0之間的差值就是末端工具的TCP在Z軸方向上的偏差：

TCP_Δz＝P_TCP.Z-P_TCP0.Z。

本發(fā)明的有益之處在于：本發(fā)明提供的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的的TCP檢測(cè)裝置及方法：一、能夠有效提高工業(yè)機(jī)器人的作業(yè)精度；二、減少工業(yè)機(jī)器人停機(jī)維護(hù)時(shí)間，提高工業(yè)生產(chǎn)線(xiàn)的效率和產(chǎn)能；三、有效地提高工業(yè)生產(chǎn)線(xiàn)的自動(dòng)化程度，節(jié)省人力、物力，降低成本。

附圖說(shuō)明

圖1是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的結(jié)構(gòu)示意圖。

圖2是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的TCP標(biāo)定裝置的結(jié)構(gòu)示意圖。

圖3是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的TCP檢測(cè)裝置的結(jié)構(gòu)示意圖。

圖4是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的標(biāo)定控制器的結(jié)構(gòu)示意圖。

圖5是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的末端工具的運(yùn)動(dòng)軌跡示意圖。

圖6是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的的TCP檢測(cè)裝置測(cè)量X/Y軸方向上位移誤差示意圖。

圖7是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的TCP檢測(cè)裝置測(cè)量X/Y軸方向上角度誤差示意圖。

圖8是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的TCP檢測(cè)裝置測(cè)量Z軸方向上位移誤差示意圖。

圖9是本發(fā)明的一種應(yīng)用于工業(yè)機(jī)器人的TCP在線(xiàn)快速標(biāo)定裝置的TCP在線(xiàn)標(biāo)定方法流程圖。

附圖中標(biāo)記的含義如下：

1、控制柜，2、TCP標(biāo)定裝置，3、工業(yè)機(jī)器人，4、末端工具，5、控制總線(xiàn)；

201、TCP檢測(cè)裝置，202、安裝底座，203、標(biāo)定控制器；

301、對(duì)射式光電傳感器1，302、對(duì)射式光電傳感器2，303、對(duì)射式光電傳感器3，304、對(duì)射式光電傳感器4，305、精度檢驗(yàn)開(kāi)關(guān)，306、上蓋，307、裝置本體；

401、裝置外殼，402、顯示單元，403、按鍵單元，404、狀態(tài)指示燈，405、通信接口。

具體實(shí)施方式

下面結(jié)合附圖和實(shí)施例，對(duì)本發(fā)明作進(jìn)一步詳細(xì)說(shuō)明。

如圖1所示：一種應(yīng)用于工業(yè)機(jī)器人3的TCP在線(xiàn)快速標(biāo)定裝置，包括控制柜1，TCP標(biāo)定裝置2，工業(yè)機(jī)器人3、末端工具4和控制總線(xiàn)5，控制柜1通過(guò)控制總線(xiàn)5分別連接TCP標(biāo)定裝置2和工業(yè)機(jī)器人3，末端工具4安裝在工業(yè)機(jī)器人3上；

如圖2所示：TCP標(biāo)定裝置2包括TCP檢測(cè)裝置201、標(biāo)定控制器203和安裝底座202，TCP檢測(cè)裝置201通過(guò)安裝底座202固定安裝在工業(yè)機(jī)器人3的一側(cè)，測(cè)量平面與工業(yè)機(jī)器人3基坐標(biāo)系的XOY平面平行。

對(duì)射式光電傳感器1-301、3-303的激光射線(xiàn)與工業(yè)機(jī)器人3基坐標(biāo)系的X軸平行。對(duì)射式光電傳感器2-302、4-304的激光射線(xiàn)與工業(yè)機(jī)器人3基坐標(biāo)系的Y軸平行。

如圖3所示：TCP檢測(cè)裝置201包括裝置上蓋306、裝置本體307、精度檢驗(yàn)開(kāi)關(guān)305以及四組對(duì)射式光電傳感器，所述精度檢驗(yàn)開(kāi)關(guān)305設(shè)置于裝置本體307上表面，所述裝置本體307為縱向鏤空正方形的長(zhǎng)方體，四組對(duì)射式光電傳感器分別設(shè)置于裝置本體307內(nèi)側(cè)面，對(duì)射式光電傳感器1-301、3-303在同一垂直平面內(nèi)，傳感器的安裝距離為d，對(duì)射式光電傳感器2-302、4-304在同一垂直平面內(nèi)，傳感器的安裝距離為d，對(duì)射式光電傳感器3-303、4-304的激光射線(xiàn)相互垂直且在同一水平面內(nèi)，對(duì)射式光電傳感器1-301、2-302的激光射線(xiàn)也相互垂直且在同一水平面內(nèi)。

如圖4所示，標(biāo)定控制器203包括裝置外殼401、微控制器單元、顯示單元 402、按鍵單元403、狀態(tài)指示燈404和通信接口405，所述標(biāo)定控制器203獲取精度檢驗(yàn)開(kāi)關(guān)305的輸出信號(hào)以及四組對(duì)射式光電傳感器的通斷信號(hào)，通過(guò)連接通信接口405的控制總線(xiàn)5與控制柜1進(jìn)行數(shù)據(jù)通信，并通過(guò)顯示單元402、狀態(tài)指示燈404反饋工作信息。

如圖9所示：一種應(yīng)用于工業(yè)機(jī)器人3的TCP在線(xiàn)快速標(biāo)定方法，其特征在于，包括如下步驟：

S1、保持工業(yè)機(jī)器人3末端工具4垂直于基坐標(biāo)系的XOY平面，通過(guò)人工示教方法實(shí)現(xiàn)工業(yè)機(jī)器人3末端工具4與TCP檢測(cè)裝置201的精度檢測(cè)開(kāi)關(guān)對(duì)準(zhǔn)，記錄工業(yè)機(jī)器人3法蘭的空間位姿數(shù)據(jù)；

S2、工業(yè)機(jī)器人3連續(xù)工作一段時(shí)間后，工業(yè)機(jī)器人3根據(jù)初始人工示教的空間位姿數(shù)據(jù)檢測(cè)當(dāng)前工業(yè)機(jī)器人3的TCP點(diǎn)是否偏移較大，若無(wú)法滿(mǎn)足生產(chǎn)要求，則進(jìn)入標(biāo)定程序；

S3、標(biāo)定程序：標(biāo)定控制器203通過(guò)控制柜1控制工業(yè)機(jī)器人3的末端工具4以初始姿態(tài)在TCP檢測(cè)裝置201中做正方形軌跡的標(biāo)定運(yùn)動(dòng)；在運(yùn)動(dòng)過(guò)程中，標(biāo)定控制器203的微控制器單元監(jiān)測(cè)TCP檢測(cè)裝置201內(nèi)的四組對(duì)射式光電傳感器通斷狀態(tài)，根據(jù)該通斷信號(hào)的時(shí)間點(diǎn)，讀取并存儲(chǔ)工業(yè)機(jī)器人3在該時(shí)間點(diǎn)的法蘭空間位姿數(shù)據(jù)，利用以上存儲(chǔ)的空間位姿數(shù)據(jù)計(jì)算工業(yè)機(jī)器人3末端工具4在X軸與Y軸方向上的位置偏差以及角度偏差，并實(shí)現(xiàn)TCP的誤差補(bǔ)償；

S4、重復(fù)2次步驟S3的操作，通過(guò)標(biāo)定結(jié)果的迭代，降低TCP標(biāo)定誤差；

S5、控制末端工具4在基坐標(biāo)系的Z軸方向上做勻速直線(xiàn)運(yùn)動(dòng)，以確定工業(yè)機(jī)器人3TCP在Z軸方向上位置偏差，補(bǔ)償TCP的誤差；

S6、重復(fù)2次步驟S5的操作，通過(guò)標(biāo)定結(jié)果的迭代，降低TCP標(biāo)定誤差。

上述的一種應(yīng)用于工業(yè)機(jī)器人3的TCP在線(xiàn)快速標(biāo)定方法，步驟S3中末端工具4在X軸與Y軸方向上的位置偏差的計(jì)算方法為：

如圖5所示，末端工具4在TCP檢測(cè)裝置201內(nèi)做正方形軌跡的標(biāo)定運(yùn)動(dòng)，在運(yùn)動(dòng)過(guò)程中前半路徑內(nèi)的末端工具4保持姿態(tài)不變，在后半路徑內(nèi)的末端工具4繞法蘭坐標(biāo)系的Z軸旋轉(zhuǎn)180°。

如圖6所示，標(biāo)定控制器203通過(guò)機(jī)器人控制柜1控制工業(yè)機(jī)器人3的末端工具4以初始姿態(tài)在TCP檢測(cè)裝置201中做正方形軌跡的標(biāo)定運(yùn)動(dòng)。在標(biāo)定運(yùn)動(dòng)過(guò)程中，末端工具4首先通過(guò)對(duì)射式光電傳感器1-301與3-303。每經(jīng)過(guò)一組對(duì)射式光電傳感器，標(biāo)定控制器203讀取工業(yè)機(jī)器人3法蘭坐標(biāo)系的空間位姿數(shù)據(jù)，記為P_ijn

所述i表示第i組對(duì)射式光電傳感器，i＝1或2或3或4，其中1為對(duì)射式光電傳感器1-301，2為對(duì)射式光電傳感器2-302，3-303為對(duì)射式光電傳感器3-303，4-304為對(duì)射式光電傳感器4-304；

所述j表示對(duì)射式光電傳感器的通斷，j＝1或2，1表示對(duì)射式光電傳感器信號(hào)接通，2表示對(duì)射式光電傳感器信號(hào)斷開(kāi)；

所述n表示單次循環(huán)運(yùn)動(dòng)中，第n次通過(guò)第i組對(duì)射式光電傳感器，n＝1或2；

基于標(biāo)定控制器203獲取的法蘭坐標(biāo)系空間位姿數(shù)據(jù)，

A、末端工具4第一次通過(guò)第1組對(duì)射式光電傳感器1-301時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器1-301的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{11}=\frac{P_{111}.Y+P_{121}.Y}{2},$

末端工具4第一次通過(guò)第3組對(duì)射式光電傳感器3-303時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器3-303的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{31}=\frac{P_{311}.Y+P_{321}.Y}{2},$

末端工具4第二次通過(guò)第1組對(duì)射式光電傳感器1-301時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器1-301的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{12}=\frac{P_{112}.Y+P_{122}.Y}{2},$

末端工具4第二次通過(guò)第3組對(duì)射式光電傳感器3-303時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器3-303的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$Y_{32}=\frac{P_{312}.Y+P_{322}.Y}{2},$

因此，末端工具4的TCP在Y軸方向上的位置偏差可由下式計(jì)算得出：

${\mathrm{TCP}}_{\mathrm{\Δ}Y}=\frac{Y_{11}-Y_{12}+Y_{31}-Y_{32}}{4};$

B、末端工具4第一次通過(guò)第2組對(duì)射式光電傳感器2-302時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器2-302的激光射線(xiàn)的交點(diǎn)X軸坐標(biāo)為：

$X_{21}=\frac{P_{211}.X+P_{221}.X}{2},$

末端工具4第一次通過(guò)第3組對(duì)射式光電傳感器4-304時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器4-304的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$X_{41}=\frac{P_{411}.X+P_{421}.X}{2},$

末端工具4第二次通過(guò)第2組對(duì)射式光電傳感器2-302時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器2-302的激光射線(xiàn)的交點(diǎn)X軸坐標(biāo)為：

$X_{22}=\frac{P_{212}.X+P_{222}.X}{2},$

末端工具4第二次通過(guò)第3組對(duì)射式光電傳感器4-304時(shí)，末端工具4的中線(xiàn)與對(duì)射式光電傳感器4-304的激光射線(xiàn)的交點(diǎn)Y軸坐標(biāo)為：

$X_{42}=\frac{P_{412}.X+P_{422}.X}{2},$

因此，計(jì)算得到末端工具4的TCP在X軸方向上的位置偏差：

${\mathrm{TCP}}_{\mathrm{\Δ}X}=\frac{X_{21}-X_{22}+X_{41}-X_{42}}{4}.$

上述步驟S3中角度偏差的計(jì)算方法為：

如圖7所示：上下兩層對(duì)射式光電傳感器1-301和3-303之間的距離為d，計(jì)算末端工具4的Y方向的角度偏差如下式所示：

${\mathrm{TCP}}_{\mathrm{\Δ}RY}=\arctan \frac{L}{d}=\arctan \left(\frac{X_{41}-X_{21}+X_{42}-X_{22}}{2d}\right),$

上下兩層對(duì)射式光電傳感器2-302和4-304之間的距離為d，計(jì)算末端工具4的X方向的角度偏差如下式所示：

${\mathrm{TCP}}_{\mathrm{\Δ}RX}=\arctan \frac{L}{d}=\arctan \left(\frac{Y_{31}-Y_{11}+Y_{32}-Y_{12}}{2d}\right).$

上述步驟S5中工業(yè)機(jī)器人3TCP在Z軸方向上位置偏差的計(jì)算方法為：

如圖8所示：標(biāo)定控制器203通過(guò)控制柜1控制工業(yè)機(jī)器人3的末端工具4運(yùn)動(dòng)到對(duì)射式光電傳感器1-301和2-302交點(diǎn)的正上方，沿著Z軸方向勻速直線(xiàn)向下運(yùn)動(dòng)，對(duì)射式光電傳感器1-301和2-302檢測(cè)到末端工具4到達(dá)兩射線(xiàn)交點(diǎn)時(shí)的位姿數(shù)據(jù)，與通過(guò)人工示教得到的初始值P_TCP0之間的差值就是末端工具4的TCP在Z軸方向上的偏差：

TCP_Δz＝P_TCP.Z-P_TCP0.Z。

在工業(yè)機(jī)器人3執(zhí)行作業(yè)任務(wù)的過(guò)程中，通過(guò)機(jī)器人控制柜1對(duì)工業(yè)機(jī)器人3的末端工具4的坐標(biāo)進(jìn)行偏差補(bǔ)償。

以上顯示和描述了本發(fā)明的基本原理、主要特征和優(yōu)點(diǎn)。本行業(yè)的技術(shù)人員應(yīng)該了解，上述實(shí)施例不以任何形式限制本發(fā)明，凡采用等同替換或等效變換的方式所獲得的技術(shù)方案，均落在本發(fā)明的保護(hù)范圍內(nèi)。