本發(fā)明主要涉及一種工業(yè)機器人示教系統，屬于機器人技術領域，特別是一種用于六軸工業(yè)機器人示教過程中的軌跡精確跟蹤示教系統。

背景技術：

六軸工業(yè)機器人是工業(yè)自動化領域的典型制造裝備，為了使工業(yè)機器人執(zhí)行搬運等工作任務，需要對其進行示教或者離線編程。目前，傳統的六軸工業(yè)機器人示教系統大部分采用間接示教方式，通過操作示教盒移動機器人到達指定位置，然后手動操作示教盒將經過的位置記錄下來，這種示教方式對操作人員的相關知識儲備要求比較高，效率低，過程復雜。而在實際應用中又產生了降低操作人員的使用門檻，簡化示教步驟，提高示教效率的市場需求。因此基于現有的六軸工業(yè)機器人開發(fā)智能示教系統具有很重要的現實意義。

論文“基于力/力矩傳感器的直接示教系統研究”，劉昆等，《自動化與儀表》，2016,31(5)，第10-14頁，公開了一種基于力/力矩傳感器的直接示教系統，該論文介紹了通過一個力/力矩傳感器感知操作者的示教力，并通過數據采集卡收集力/力矩的電壓模擬信號，根據6個方向模擬電壓的變化轉化成其各個方向的位移，并于示教過程中通過示教盒手動記錄特征軌跡點。然而，該系統并未涉對牽引力方向的計算與預測算法，僅僅把6個方向模擬電壓的變化轉化成其各個方向的位移，當牽引力方向與三個坐標軸成一定角度時，會出現較大的位置偏差。同時該系統仍然使用傳統的數據采集卡與上位機(PC)的模式，系統占用空間大，可移植性弱。因此，適時適當設計一種基于嵌入式系統、具備牽引力方向預測與人機互動功能的智能示教系統非常有必要。

論文“基于順應性跟蹤控制的工業(yè)機器人直接示教系統”，徐建明等，《高技術通訊》，2015,25(5)，第500-507頁，公開了一種基于順應性跟蹤控制的工業(yè)機器人直接示教系統，該系統基于力偏差信號調節(jié)機械臂末端執(zhí)行器的運動速度，實現機械臂對牽引力的順應性跟蹤。該系統未涉及對受力方向的計算與預測，僅僅通過力控制器調節(jié)機械臂末端執(zhí)行器的參考速度，因此在當牽引力方向與三個坐標軸成一定角度時，仍會出現較大的位置偏差，不能實現真正的順應性。同時該系統不能通過人機互動對示教位置快速記錄。

專利CN103213136A公開了一種用于工業(yè)機器人牽引示教的方法和系統，該專利提出了一種基于三維壓力傳感器和三維位姿傳感器的手持式示教裝置。該裝置僅涉及如何改進力傳感器獲取壓力信號的方式，在對壓力信號的處理算法上仍然使用傳統的根據壓力差的大小調整運動速度，并沒有對所施加的力方向進行計算與分析，因此會出現工業(yè)機器人控制器仍然不知道下一個目標點的位姿坐標，不能實現真正的順應性牽引示教。也沒有通過人機互動對示教的位置進行實時記錄的功能。

專利CN105058396A公開了一種機器人示教系統及其控制方法，該專利所述機器人示教系統包括：傳感模塊、機器人控制器及上位機；其中傳感模塊包括力傳感器、力傳感器調制器以及體感捕捉設備。該系統實際上是通過體感捕捉設備獲取操作者的指令，再將指令傳送給機器人控制器，由機器人控制器執(zhí)行相應的位姿調整指令。力傳感器僅僅作為一個檢測裝置，并不是通過力傳感器獲取牽引指令，進而使機器人做出順應性調整。

技術實現要素：

本發(fā)明的目的在于針對上述傳統的六軸工業(yè)機器人不能直接而柔順地牽引機器人末端執(zhí)行器到達指定空間位置，實時與機器人交互，將經過位置數據存儲的問題。當操作六軸機器人示教盒進行示教時能直接牽引機器人末端，使機器人跟隨牽引力到達預定的位置；在牽引的過程中能自動的計算并預測機器人末端所施加的牽引力的方向，六軸機器人末端自動做出位姿調整，不僅使牽引軌跡更加平滑而且控制精度更高，示教過程花費時間更短；其次，六維力傳感器可以感知操作人員對其輸入的指令，使六軸工業(yè)機器人控制器自動的記錄當前位置，并隨著指令的改變，能自動切換到抓取功能，操作簡單。最后，通信基于工業(yè)以太網Ethernet進行數據交換，數據引入了嵌入式系統進行處理，使得本智能示教系統還具有響應時間少、實用性強、可移植性應用價值高等特點。

為實現上述目的，本發(fā)明采用的技術方案為一種基于順應性跟蹤的六軸工業(yè)機器人智能示教系統，該示教系統包括六維力信號感知系統、力方向解算模塊模塊、六軸工業(yè)機器人系統、動作執(zhí)行模塊；六維力信號感知系統包括圓形載荷單元(1)、法蘭連接結構(2)、力信號數據輸入接口(3)數據存儲盒(4)、力信號數據輸出接口(5)、Ethernet通信網線(6)；

力方向解算模塊包括樹莓派Ethernet輸入接口(7)、基于linux的嵌入式控制系統樹莓派(8)、六維力信號解耦單元(9)、力坐標變換處理單元(10)、樹莓派IO數據輸出接口(11)、繼電器(12)、路由器(13)；

六軸工業(yè)機器人系統包括六軸工業(yè)機器人控制器(14)、六軸工業(yè)機器人控制器IO數據輸入單元(15)、機器人控制器Ethernet輸入接口(16)、運動學逆解計算單元(17)、運動驅動單元(18)；

動作執(zhí)行模塊包括六軸工業(yè)機器人機械本體(19)，氣動抓手(20)，底座(21)。

六維力傳感器的圓形載荷單元(1)作為整個系統的直接受力單元感獲取外界受力情況信息，圓形載荷單元(1)與法蘭連接結構(2)連接，從而與六軸機器人本體(19)末端進行連接；圓形載荷單元(1)通過電纜線與力信號數據輸入接口(3)連接，通過力信號數據輸入接口(3)將數據存儲到數據存儲盒(4)，數據存儲盒(4)將數據發(fā)送到力信號數據輸出接口(5)；通過Ethernet通信網線(6)與路由器(13)連接；

在力方向解算模塊中，基于linux嵌入式控制系統的樹莓派(8)設置有六維力信號解耦單元(9)、力坐標變換處理單元(10)兩個功能單元，樹莓派(8)通過以太網線將其Ethernet輸入接口(7)接入路由器(13)；樹莓派IO數據輸出接口(11)與繼電器(12)連接；

六軸工業(yè)機器人控制器(14)設置有運動學逆解計算單元(17)、運動驅動單元(18)，繼電器(12)另一端與六軸工業(yè)機器人控制器(14)的IO數據輸入單元(15)連接；機器人控制器Ethernet輸入接口(16)通過以太網線接入路由器(13)。

六軸工業(yè)機器人機械本體(19)通過電纜線與六軸工業(yè)機器人控制器(14)連接，在六軸工業(yè)機器人機械本體(19)的末端通過法蘭連接圓形載荷單元(1)再連接氣動抓手(20)；六軸工業(yè)機器人機械本體(19)的底部固定在底座(21)上。

本發(fā)明的有益效果是：由于本發(fā)明設計了一種力方向感知與預測的算法，使工業(yè)機器人末端沿著該方向做出相應的調整，從而使牽引軌跡更加平滑，控制精度更高，實現了人與工業(yè)機器人的柔性互動；其次，本發(fā)明采用了工業(yè)以太網Ethernet作為通信總線，因此系統內的各個組件都被賦予了IP地址，有利于實現機器人物聯網。引入嵌入式系統樹莓派作為力信號處理模塊，減小了整個智能示教系統的體積，可靠穩(wěn)定，可移植性強，適用范圍廣。最后，采用了六維力傳感器檢測六軸工業(yè)機器人末端力信號的輸入，賦予了示教系統感知人的命令功能，六軸工業(yè)機器人根據不同的力信號輸入命令分別可以進行位移變換與角度變換的轉變、快速記錄當前位置，以及控制末端氣動抓手等動作，賦予了六軸工業(yè)機器人更加智能化的功能。

附圖說明

圖1為智能示教系統的主體結構。

圖2為六軸工業(yè)機器人系統結構。

圖3為本智能示教系統控制方法軟件執(zhí)行流程圖。

圖4為本發(fā)明與傳統示教系統的牽引效果對比圖。

圖中：1、圓形載荷單元，2、法蘭連接結構，3、力信號數據輸入接口，4、數據存儲盒，5、力信號數據輸出接口，6、Ethernet通信網線，7、樹莓派Ethernet輸入接口，8、基于linux的嵌入式控制系統樹莓派，9、六維力信號解耦單元，10、力坐標變換處理單元，11、樹莓派IO數據輸出接口，12、繼電器，13、路由器，14、六軸工業(yè)機器人控制器、15、六軸工業(yè)機器人控制器IO數據輸入單元，16、機器人控制器Ethernet輸入接口，17、運動學逆解計算單元，18、運動驅動單元，19、六軸工業(yè)機器人機械本體，20、氣動抓手，21、底座。

具體實施方式

如圖1-4所示，首先，六軸工業(yè)機器人控制器(14)與基于linux的嵌入式控制系統樹莓派(8)完成參數初始化，圓形載荷單元(1)檢測機器人末端受力情況，受力情況再通過力信號數據輸入接口(3)發(fā)送到數據存儲盒(4)進行動態(tài)存儲，然后通過力信號數據輸出接口(5)與Ethernet通信網線(6)接入路由器(13)，與此同時，數據存儲盒(4)將獲得一個IP地址。

其次，力信號通過路由器(13)端口的傳遞后從樹莓派Ethernet輸入接口(7)進入基于linux的嵌入式控制系統樹莓派(8)，在系統樹莓派中，力信號要經過六維力信號解耦單元(9)、力坐標變換處理單元(10)的信號處理過程，進而獲得六軸工業(yè)機器人末端的六個維度的受力信息。獲得六個維度的力信息后，一方面系統樹莓派將處理信息通過樹莓派IO數據輸出接口(11)發(fā)送至繼電器(12)，再通過六軸工業(yè)機器人控制器IO數據輸入單元輸入六軸工業(yè)機器人控制器(14)，從而計算出受力的方向在以工具中心位置(TCP)為原點的空間坐標系八個象限中的位置。另一方面，通過方向預測算法，將六個維度的力大小通過數學運算，獲得受力方向單位向量與X、Y、Z軸正方向的夾角α、β、γ，再通過剛度系數矩陣，將受力的大小轉換為對應機器人末端調整位移d，從而通過這四個參數及方向預測算法，運算出機器人末端下一個目標點的空間位置坐標。最后將下一個目標點的空間位置坐標通過樹莓派Ethernet輸入接口(7)傳輸到路由器(13)，此時路由器(13)已經分別為樹莓派(8)與六軸工業(yè)機器人控制器(14)分配了IP地址，因此下一個目標點的空間位置坐標順利傳遞到六軸工業(yè)機器人控制器(14)。

最后，六軸工業(yè)機器人控制器(14)通過運動學逆解計算單元(17)將一個目標點的空間位置坐標轉換為六個軸的相應的轉角，再經過運動驅動單元(18)使得六個關節(jié)的電機獲得扭矩，從而使六軸工業(yè)機器人末端做出了相應的位置與姿態(tài)的調整，從而實現六軸工業(yè)機器人末端在牽引力作用下的順應性跟蹤功能。

當牽引機器人末端到達操作空間的某一位置時，如果想調整當前的姿態(tài)，通過在氣動抓手(20)的末端輕輕敲擊一下，此時六維力傳感器將獲得一次短暫的敲擊信號，此時敲擊信號將轉換為繞Y軸的轉矩輸入，通過樹莓派(8)的處理后，通過樹莓派IO數據輸出接口(11)發(fā)送至繼電器(12)，再通過六軸工業(yè)機器人控制器IO數據輸入單元輸入六軸工業(yè)機器人控制器(14)，機器人控制器(14)將獲得一次位置與姿態(tài)轉換的命令。

當六軸工業(yè)機器人的位置和姿態(tài)在牽引力的作用下調整好后，如果想記錄當前位置，只需輕輕敲擊氣動抓手(20)的末端兩次，此時敲擊信號將轉換為繞Y軸的兩次轉矩輸入，通過樹莓派(8)的處理后，通過樹莓派IO數據輸出接口(11)發(fā)送至繼電器(12)，再通過六軸工業(yè)機器人控制器IO數據輸入單元輸入六軸工業(yè)機器人控制器(14)，機器人控制器(14)將獲得一次記錄當前位置的命令。機器人控制器通過將當前位置數據輸出到一個文本文件中，并實時保存，到達下一個記錄位置時執(zhí)行相同的過程來記錄機器人的位置與姿態(tài)。

當到達某個位置要執(zhí)行抓取動作時，只需輕輕敲擊氣動抓手(20)的末端三次，此時敲擊信號將轉換為繞Y軸的兩次轉矩輸入，通過樹莓派(8)的處理后，通過樹莓派IO數據輸出接口(11)發(fā)送至繼電器(12)，再通過六軸工業(yè)機器人控制器IO數據輸入單元輸入六軸工業(yè)機器人控制器(14)，機器人控制器(14)將獲得一次氣動抓手開或者關的命令。從而控制氣動抓手執(zhí)行相應的抓取動作。

樹莓派(8)基于linux操作系統，其供電為5V直流電源。樹莓派作為本系統的控制核心，用于實現力信息采集、信號變換、力傳感器六個通道的數據解耦、坐標變換以及方向預測算法。

與圓形載荷單元(1)連接的數據存儲盒(4)采用Sunrise M8128作為模數轉換的核心元件，將采集到的六個通道的模擬信號轉換成數字量，通過Ethernet接口送樹莓派處理。

系統通電后，首先對系統進行初始化，掃描力輸入信號，等待啟動鍵輸入。當有力信號輸入時，系統將進入力方向解算模塊。

力方向解算模塊中，使用的六維傳感器采用矩陣解耦算法實現解耦，計算步驟如下：

1)獲取通道1至通道6的原始數據

假設獲得的原始數據向量為：

RAW＝[rawchn1,rawchn2,rawchn3,rawchn4,rawchn5,rawchn6] (1)

上式中rawchn1,rawchn2,rawchn3,rawchn4,rawchn5,rawchn6分別代表從圓形載荷單元上采集到的沿X軸、Y軸、Z軸和繞X軸、Y軸、Z軸六個維度上的模擬電壓的信號經過模數轉換后的數字電壓信號，單位為伏特V。

2)將原始數轉化為以mV/V為單位的數據

激勵電壓大小為EXC＝5.002240，六維力傳感器六個通道上的增益常量為Gain＝[gain1,gain2,gain3,gain4,gain5,gain6]，其中gain1,gain2,gain3,gain4,gain5,gain6分別代表每個通道上的增益常量，六維力傳感器6個通道上的放大器零點偏移為AmpZero＝[AZ1,AZ2,AZ3,AZ4,AZ5,AZ6]，其中AZ1,AZ2,AZ3,AZ4,AZ5,AZ6分別代表每個通道上的放大器零點偏移，由以上數據得到：

[DAT]＝1000*(RAW-AmpZero)/65535*5/Gain/EXC (2)

3)計算解耦載荷

由傳感器的類型確定解耦矩陣[Decoupled]，它是一個常量，需要通過對傳感器進行標定后得到，本發(fā)明所采用的解耦矩陣為

最后，得到解耦后的載荷為：

[Result]＝[Decoupled]*[DAT]＝[FX,FY,FZ,MX,MY,MZ] (3)

上式中FX,FY,FZ分別代表原始數據經解耦后沿X軸、Y軸、Z軸方向的力，MX,MY,MZ分別代表原始數據經解耦后繞X軸、Y軸、Z軸方向的力矩。

力坐標變換與力方向預測算法如下：

通過公式(4)將傳感器上的受力信息轉換為機器人末端受力信息。F_d代表機器人末端工具中心位置(TCP)受力大小，F_i代表六維力傳感器載荷單元受力大小，J代表雅克比矩陣，表示從關節(jié)速度到操作空間速度的廣義傳動比，(J^T)^-1表示雅克比矩陣的轉置矩陣再求逆矩陣

F_d＝(J^T)^-1F_i (4)

為了獲得六軸工業(yè)機器人控制器所要執(zhí)行的下一位置的信息，設機器人末端TCP位置為P₁(x₁,y₁,z₁),牽引力所指向的下一位置為P₂(x₂,y₂,z₂)，則有P₁(x₁,y₁,z₁)與P₂(x₂,y₂,z₂)之間的距離d為：

設每次輸入機器人控制器的位置增量Δ_x＝d，以機器人末端TCP位置P₁(x₁,y₁,z₁)為坐標原點，若要計算出該方程需要知道P₂(x₂,y₂,z₂)的各個坐標，根據牽引力的解耦數據[FX,FY,FZ,MX,MY,MZ]，獲得各個分力的大小，進而根據公式(6)計算出牽引力及其投影與X,Y,Z軸正方向的夾角α,β,γ

并由此得到計算出下一位置的公式如下：

將獲得的下一位置的坐標P₂(x₂,y₂,z₂)，通過Ethernet總線的方式傳輸到六軸工業(yè)機器人控制器，機器人控制器通過程序驅動各個關節(jié)電機到達P₂(x₂,y₂,z₂)，從而實現位置調整與六軸工業(yè)機器人的順應性跟蹤控制。