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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

六軸工業(yè)機(jī)器人系統(tǒng)包括六軸工業(yè)機(jī)器人控制器(14)、六軸工業(yè)機(jī)器人控制器IO數(shù)據(jù)輸入單元(15)、機(jī)器人控制器Ethernet輸入接口(16)、運(yùn)動(dòng)學(xué)逆解計(jì)算單元(17)、運(yùn)動(dòng)驅(qū)動(dòng)單元(18)；

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

六維力傳感器的圓形載荷單元(1)作為整個(gè)系統(tǒng)的直接受力單元感獲取外界受力情況信息，圓形載荷單元(1)與法蘭連接結(jié)構(gòu)(2)連接，從而與六軸機(jī)器人本體(19)末端進(jìn)行連接；圓形載荷單元(1)通過(guò)電纜線與力信號(hào)數(shù)據(jù)輸入接口(3)連接，通過(guò)力信號(hào)數(shù)據(jù)輸入接口(3)將數(shù)據(jù)存儲(chǔ)到數(shù)據(jù)存儲(chǔ)盒(4)，數(shù)據(jù)存儲(chǔ)盒(4)將數(shù)據(jù)發(fā)送到力信號(hào)數(shù)據(jù)輸出接口(5)；通過(guò)Ethernet通信網(wǎng)線(6)與路由器(13)連接；

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

六軸工業(yè)機(jī)器人控制器(14)設(shè)置有運(yùn)動(dòng)學(xué)逆解計(jì)算單元(17)、運(yùn)動(dòng)驅(qū)動(dòng)單元(18)，繼電器(12)另一端與六軸工業(yè)機(jī)器人控制器(14)的IO數(shù)據(jù)輸入單元(15)連接；機(jī)器人控制器Ethernet輸入接口(16)通過(guò)以太網(wǎng)線接入路由器(13)。

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

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

附圖說(shuō)明

圖1為智能示教系統(tǒng)的主體結(jié)構(gòu)。

圖2為六軸工業(yè)機(jī)器人系統(tǒng)結(jié)構(gòu)。

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

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

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

具體實(shí)施方式

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

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

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

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

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

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

樹(shù)莓派(8)基于linux操作系統(tǒng)，其供電為5V直流電源。樹(shù)莓派作為本系統(tǒng)的控制核心，用于實(shí)現(xiàn)力信息采集、信號(hào)變換、力傳感器六個(gè)通道的數(shù)據(jù)解耦、坐標(biāo)變換以及方向預(yù)測(cè)算法。

與圓形載荷單元(1)連接的數(shù)據(jù)存儲(chǔ)盒(4)采用Sunrise M8128作為模數(shù)轉(zhuǎn)換的核心元件，將采集到的六個(gè)通道的模擬信號(hào)轉(zhuǎn)換成數(shù)字量，通過(guò)Ethernet接口送樹(shù)莓派處理。

系統(tǒng)通電后，首先對(duì)系統(tǒng)進(jìn)行初始化，掃描力輸入信號(hào)，等待啟動(dòng)鍵輸入。當(dāng)有力信號(hào)輸入時(shí)，系統(tǒng)將進(jìn)入力方向解算模塊。

力方向解算模塊中，使用的六維傳感器采用矩陣解耦算法實(shí)現(xiàn)解耦，計(jì)算步驟如下：

1)獲取通道1至通道6的原始數(shù)據(jù)

假設(shè)獲得的原始數(shù)據(jù)向量為：

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

上式中rawchn1,rawchn2,rawchn3,rawchn4,rawchn5,rawchn6分別代表從圓形載荷單元上采集到的沿X軸、Y軸、Z軸和繞X軸、Y軸、Z軸六個(gè)維度上的模擬電壓的信號(hào)經(jīng)過(guò)模數(shù)轉(zhuǎn)換后的數(shù)字電壓信號(hào)，單位為伏特V。

2)將原始數(shù)轉(zhuǎn)化為以mV/V為單位的數(shù)據(jù)

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

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

3)計(jì)算解耦載荷

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

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

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

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

力坐標(biāo)變換與力方向預(yù)測(cè)算法如下：

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

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

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

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

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

將獲得的下一位置的坐標(biāo)P₂(x₂,y₂,z₂)，通過(guò)Ethernet總線的方式傳輸?shù)搅S工業(yè)機(jī)器人控制器，機(jī)器人控制器通過(guò)程序驅(qū)動(dòng)各個(gè)關(guān)節(jié)電機(jī)到達(dá)P₂(x₂,y₂,z₂)，從而實(shí)現(xiàn)位置調(diào)整與六軸工業(yè)機(jī)器人的順應(yīng)性跟蹤控制。