房立金，許繼謙，高 躍，趙乾坤，郭 鋮，錢奕安

（1.東北大學(xué)機(jī)器人科學(xué)與工程學(xué)院，沈陽(yáng) 110819；2.東北大學(xué)機(jī)械工程與自動(dòng)化學(xué)院，沈陽(yáng) 110819）

一般的工業(yè)機(jī)械臂在進(jìn)行裝配操作時(shí)，經(jīng)常會(huì)遇到精度不高的問題。目前針對(duì)這一問題的普遍解決方法是額外制作一套高精度的執(zhí)行機(jī)構(gòu)安裝在機(jī)械臂末端，進(jìn)行位置的矯正并提高作業(yè)精度。

盡管采用諧波或RV 減速器極大地提高了機(jī)器人的精度，但機(jī)器人長(zhǎng)時(shí)間運(yùn)行時(shí)存在磨損，在機(jī)械臂高速運(yùn)行狀態(tài)下，傳動(dòng)間隙的影響依然存在，可能會(huì)引起系統(tǒng)震蕩從而導(dǎo)致系統(tǒng)無法工作，嚴(yán)重時(shí)可能會(huì)損壞設(shè)備結(jié)構(gòu)[1]。

此外，目前用于裝配的機(jī)械臂的電機(jī)、減速器等零件單價(jià)較高，且經(jīng)過長(zhǎng)期使用磨損后精度降低，如果更換減速器，則會(huì)造成機(jī)器人使用成本的增加。

針對(duì)以上問題，本文提出一種多電機(jī)共軸驅(qū)動(dòng)的七自由度機(jī)械臂，在機(jī)械臂的腰、肩、肘關(guān)節(jié)處布置了雙電機(jī)共軸驅(qū)動(dòng)的關(guān)節(jié)結(jié)構(gòu)。一方面，可通過電機(jī)的消隙控制提高機(jī)械臂自身的運(yùn)動(dòng)精度；另一方面可以通過電機(jī)的同步控制提高機(jī)械臂的負(fù)載能力。文中提出一種機(jī)械臂的主要結(jié)構(gòu)以及針對(duì)多電機(jī)驅(qū)動(dòng)的消隙和同步控制的方法，并基于ROS 和EtherCAT總線技術(shù)，給出了機(jī)械臂控制系統(tǒng)的總體設(shè)計(jì)。

1 總體結(jié)構(gòu)設(shè)計(jì)

機(jī)械臂采用七自由度結(jié)構(gòu)，利用冗余自由度的自運(yùn)動(dòng)特性，克服傳統(tǒng)六軸工業(yè)機(jī)器人在避障、運(yùn)動(dòng)靈活性等方面的缺點(diǎn)，提高機(jī)械臂運(yùn)動(dòng)的靈活性。

雙電機(jī)共軸驅(qū)動(dòng)的目的主要有兩個(gè)方面：一方面是消除傳動(dòng)間隙對(duì)機(jī)器人精度的影響；另一方面是在一定程度上提升機(jī)器人的負(fù)載能力?；诖?，在機(jī)械臂的腰關(guān)節(jié)、肩關(guān)節(jié)、肘關(guān)節(jié)等大負(fù)載關(guān)節(jié)，采用雙電機(jī)伺服驅(qū)動(dòng)技術(shù)。其他關(guān)節(jié)采用帶有諧波減速器的關(guān)節(jié)結(jié)構(gòu)，實(shí)現(xiàn)輕載關(guān)節(jié)的輕量、緊湊結(jié)構(gòu)設(shè)計(jì)，總體上兼顧了機(jī)械臂的負(fù)載能力、緊湊性及靈活性。機(jī)械臂樣機(jī)的總體結(jié)構(gòu)如圖1所示，結(jié)構(gòu)簡(jiǎn)圖見圖2，D–H 參數(shù)見表1。

表1 機(jī)械臂的D–H 參數(shù)Table 1 D–H parameters of robotic arm

圖1 機(jī)械臂樣機(jī)Fig.1 Robotic arm

圖2 機(jī)械臂結(jié)構(gòu)圖Fig.2 Robotic arm structure diagram

2 支撐軸設(shè)計(jì)

雙電機(jī)共軸驅(qū)動(dòng)通過兩臺(tái)驅(qū)動(dòng)電機(jī)按照對(duì)稱結(jié)構(gòu)帶動(dòng)兩個(gè)驅(qū)動(dòng)輪共同驅(qū)動(dòng)從動(dòng)輪，在伺服系統(tǒng)啟動(dòng)或換向的過程中，對(duì)兩臺(tái)驅(qū)動(dòng)電機(jī)的輸出轉(zhuǎn)矩進(jìn)行聯(lián)動(dòng)控制，使得至少有一個(gè)驅(qū)動(dòng)輪與從動(dòng)輪相嚙合，保證系統(tǒng)無間隙傳動(dòng)。

在結(jié)構(gòu)設(shè)計(jì)中，利用雙電機(jī)驅(qū)動(dòng)漸開線齒輪傳動(dòng)機(jī)構(gòu)來代替高精度的RV 減速器或諧波減速器來實(shí)現(xiàn)關(guān)節(jié)的高精度傳動(dòng)。

如圖3所示，關(guān)節(jié)上包括兩組伺服電機(jī)和行星減速機(jī)構(gòu)，驅(qū)動(dòng)部件均安裝在支撐座內(nèi)部，兩組電機(jī)對(duì)稱布置在支撐座的左右兩端，并分別與共軸驅(qū)動(dòng)齒輪組嚙合，輸出端添加了絕對(duì)位置編碼器。共軸驅(qū)動(dòng)齒輪安裝有力矩傳感器，用以測(cè)量關(guān)節(jié)力矩，經(jīng)過連接法蘭與采用交叉滾子軸承支撐的輸出軸法蘭相連接。雙電機(jī)系統(tǒng)的傳動(dòng)原理如圖4所示[2]。

圖3 雙電機(jī)共軸關(guān)節(jié)結(jié)構(gòu)示意圖Fig.3 Schematic diagram of dual-motor coaxial joint structure

圖4 雙電機(jī)伺服驅(qū)動(dòng)關(guān)節(jié)原理[2]Fig.4 Principle of dual-motor servo drive joints[2]

所設(shè)計(jì)的雙電機(jī)關(guān)節(jié)均采用了較為常規(guī)的電機(jī)與減速器，相比單電機(jī)關(guān)節(jié)的高精度機(jī)械臂所采用的電機(jī)與減速器，更有利于降低機(jī)器人的使用成本。

3 雙電機(jī)驅(qū)動(dòng)系統(tǒng)的運(yùn)行模式劃分

雙電機(jī)消隙傳動(dòng)方式的基本原理是控制雙電機(jī)的輸出力矩，既要保證跟蹤目標(biāo)軌跡，又要保證任意時(shí)刻至少有一個(gè)驅(qū)動(dòng)齒輪與負(fù)載齒輪嚙合，在此基礎(chǔ)上最大限度地提高雙電機(jī)的能量利用率。根據(jù)雙電機(jī)原理劃分出相應(yīng)的狀態(tài)或模式進(jìn)行控制。

為了更好地分析雙電機(jī)驅(qū)動(dòng)系統(tǒng)的工作情況，本文依據(jù)消隙過程中的齒輪嚙合情況，將齒輪嚙合狀態(tài)劃分為異側(cè)齒面嚙合狀態(tài) （圖5（a），狀態(tài)DS）和同側(cè)齒面嚙合狀態(tài)（圖5（b），狀態(tài)SS）。同側(cè)齒面嚙合由正向齒面嚙合和反向齒面嚙合兩種不同的狀態(tài)組成。將正向同側(cè)齒面嚙合的狀態(tài)稱為正向狀態(tài)SS，將反向同側(cè)齒面嚙合的狀態(tài)稱為反向狀態(tài)SS。因此，可以將消隙過程用“狀態(tài)DS”、“正向狀態(tài)SS”、“反向狀態(tài)SS”來描述。圖5中，T1、T2為扭矩；ω為角速度；Tb為負(fù)載力矩；Td為擾動(dòng)力矩。Tb、Td的方向和大小有多種不同的組合。

圖5 輪齒嚙合狀態(tài)Fig.5 Gear tooth meshing state

綜合考察系統(tǒng)從靜止、正向加速、勻速運(yùn)動(dòng)、減速、反向加速、勻速運(yùn)動(dòng)、減速再到靜止的整個(gè)作用過程，可以按照以下狀態(tài)對(duì)其進(jìn)行描述。

（1）自由靜止?fàn)顟B(tài)：系統(tǒng)未受到任何作用，系統(tǒng)靜止。

（2）狀態(tài)DS：主從電機(jī)輸出大小相等、方向相反的力矩消除間隙，系統(tǒng)靜止。

（3）狀態(tài)DS：主電機(jī)輸出正向驅(qū)動(dòng)力，從電機(jī)輸出反向?qū)寡a(bǔ)償力矩，系統(tǒng)加速。

（4）正向狀態(tài)SS：主從電機(jī)共同輸出正向驅(qū)動(dòng)力矩，系統(tǒng)加速。

（5）狀態(tài)DS：主從電機(jī)力矩達(dá)到平衡狀態(tài)，系統(tǒng)勻速運(yùn)動(dòng)。

（6）狀態(tài)DS：從電機(jī)輸出反向制動(dòng)力矩，主電機(jī)輸出正向?qū)寡a(bǔ)償力矩，系統(tǒng)減速以及反向加速。

（7）反向狀態(tài)SS：主從電機(jī)共同輸出反向力矩，系統(tǒng)反向加速。

（8）狀態(tài)DS：主從電機(jī)力矩再次達(dá)到平衡狀態(tài)，系統(tǒng)反向勻速運(yùn)動(dòng)。

（9）狀態(tài)DS：主電機(jī)輸出正向制動(dòng)力矩，從電機(jī)輸出反向?qū)寡a(bǔ)償力矩，系統(tǒng)減速。

（10）正向狀態(tài)SS：主從電機(jī)共同輸出減速制動(dòng)力矩，系統(tǒng)減速。

（11）狀態(tài)DS：從電機(jī)輸出制動(dòng)力矩，主電機(jī)輸出對(duì)抗力矩，處于力矩平衡狀態(tài)，系統(tǒng)靜止。

上述雙電機(jī)消隙過程是動(dòng)態(tài)的，需要實(shí)時(shí)動(dòng)態(tài)地調(diào)整兩臺(tái)電機(jī)的輸出力矩，系統(tǒng)需要經(jīng)歷從狀態(tài)DS 和狀態(tài)SS 之間的多次切換過渡。

上述狀態(tài)之間的切換需要根據(jù)位置控制目標(biāo)、負(fù)載狀態(tài)以及電機(jī)當(dāng)前狀態(tài)等綜合情況來進(jìn)行，控制過程比較復(fù)雜，采取簡(jiǎn)單的控制手段難以完成兩臺(tái)電機(jī)輸出力矩的協(xié)調(diào)控制。

雙電機(jī)驅(qū)動(dòng)消隙方法具有以下優(yōu)點(diǎn)[1]。

（1）能夠有效消除傳動(dòng)間隙所導(dǎo)致的定位精度誤差，即使齒輪間隙由于系統(tǒng)長(zhǎng)時(shí)間運(yùn)行增大后，也不會(huì)影響系統(tǒng)的定位精度。

（2）采用普通精度的減速齒輪箱代替了具有機(jī)械消隙功能的高精度減速機(jī)構(gòu)，減小了驅(qū)動(dòng)系統(tǒng)的成本，不需要定期調(diào)整機(jī)械消隙機(jī)構(gòu)，維護(hù)成本降低。

（3）應(yīng)用兩套伺服驅(qū)動(dòng)系統(tǒng)共同承擔(dān)系統(tǒng)負(fù)載，每個(gè)伺服電機(jī)僅負(fù)擔(dān)最大功率的1/2，可以選擇較小的伺服電機(jī)和伺服驅(qū)動(dòng)器。

（4）采用雙電機(jī)消隙方法可以有效解決伺服電機(jī)驅(qū)動(dòng)負(fù)載頻繁換向，難以克服由間隙造成的瞬態(tài)誤差的問題。

4 消隙與同步控制算法

4.1 消隙控制總體方案

雙電機(jī)系統(tǒng)的變偏置力矩控制原理如圖6所示（其中，nset為設(shè)定轉(zhuǎn)速；nact1為電機(jī)1 轉(zhuǎn)速；nact2為電機(jī)2轉(zhuǎn)速）。

主動(dòng)軸與從動(dòng)軸電機(jī)均采用速度控制方式，在電機(jī)速度控制的基礎(chǔ)上，建立位置控制環(huán)[3]。其中PI 控制器能夠均衡擾動(dòng)負(fù)載，補(bǔ)償電壓控制器，同時(shí)控制主從軸之間的偏置力矩。主從軸電流調(diào)節(jié)器的輸入信號(hào)插值經(jīng)過PI 控制器運(yùn)算輸出，作為附加的速度以相反符號(hào)分別反饋至主從軸的速度設(shè)定點(diǎn)，調(diào)節(jié)主從軸間的扭矩平衡分配形成力矩同步控制。

消隙偏置電壓Ubias經(jīng)過濾波器和PI 控制器后，作為設(shè)定的速度，分別以相反的符號(hào)作用于主軸和從軸，間接作用于電流調(diào)節(jié)器，使得主從軸之間形成偏置力矩來消除反向間隙。

4.2 消隙偏置電壓計(jì)算與轉(zhuǎn)換函數(shù)設(shè)計(jì)

圖6中虛線框內(nèi)即為雙電機(jī)偏置電壓的計(jì)算過程，將實(shí)時(shí)檢測(cè)的負(fù)載電流取絕對(duì)值后，選擇較大的電流值作為轉(zhuǎn)換函數(shù)的輸入值，輸入電流iabs為

圖6 雙電機(jī)變偏置力矩控制結(jié)構(gòu)框圖[3]Fig.6 Block diagram of dual-motor variable bias torque control structure[3]

式中，i1和i2分別為電機(jī)1 和2 的負(fù)載電流。

為了能夠?qū)崿F(xiàn)消隙控制與同步控制的切換，控制器中引入了一個(gè)轉(zhuǎn)換函數(shù)w，即

式中，iset1、iset2分別為轉(zhuǎn)換函數(shù)中電流設(shè)定值。

Uconst為設(shè)定的補(bǔ)償電壓；Uconst與w相乘后形成動(dòng)態(tài)偏置電壓Ubias，Ubias∈[0，Uconst]，偏置電壓值疊加作用在速度環(huán)給定處，在電機(jī)的輸出端形成偏置力矩Tbias∈[0，T0]；T0為補(bǔ)償電壓Uconst對(duì)應(yīng)的偏置力矩值。根據(jù)偏置電壓的計(jì)算方法，可以計(jì)算出偏置力矩控制下，雙電機(jī)驅(qū)動(dòng)系統(tǒng)正向運(yùn)動(dòng)時(shí)電機(jī)輸出力矩的大小。

電機(jī)1 和2 的輸出力矩分別為T1和T2，Tc為位置控制器算出的轉(zhuǎn)矩控制量，可根據(jù)對(duì)抗程度進(jìn)行選擇。電機(jī)反向驅(qū)動(dòng)時(shí)，電機(jī)的力矩與正向驅(qū)動(dòng)同理，由以上分析得知，系統(tǒng)運(yùn)動(dòng)過程中，兩電機(jī)在任意時(shí)刻輸出的力矩均滿足響應(yīng)的力矩分配關(guān)系，滿足消隙控制階段、消隙控制與共同驅(qū)動(dòng)相互轉(zhuǎn)換的過渡階段和共同驅(qū)動(dòng)控制階段的力矩輸出要求。

雙電機(jī)系統(tǒng)中電機(jī)之間形成的偏置力矩是消除傳動(dòng)間隙的關(guān)鍵，不同工作狀態(tài)下的消隙作用與偏置力矩的關(guān)系較為復(fù)雜，為了應(yīng)用方便并保證消隙效果，工程中常選擇較大的偏置力矩值，通常選擇電機(jī)額定扭矩的10%～30%作為消隙補(bǔ)償力矩[2]。

采用的同步控制方法可以使系統(tǒng)輸出較大的偏置力矩用于消隙，同時(shí)降低靜態(tài)偏置力矩減小能耗，并在無間隙表現(xiàn)時(shí)具備大力矩輸出能力，通過設(shè)置合理的電流設(shè)定值，即可得到相應(yīng)的轉(zhuǎn)換函數(shù)。

4.3 同步控制

在雙永磁同步電機(jī)系統(tǒng)中，常會(huì)因?yàn)閭鲃?dòng)鏈的扭轉(zhuǎn)剛度特性差異和電機(jī)所受的負(fù)載擾動(dòng)的不同，電機(jī)之間存在一定概率的轉(zhuǎn)速差，剛性連接非常容易激發(fā)差速震蕩從而影響系統(tǒng)的穩(wěn)定性，嚴(yán)重時(shí)還可能會(huì)損壞設(shè)備結(jié)構(gòu)[4]。因此，需要提高控制系統(tǒng)的抗干擾性，從而減小驅(qū)動(dòng)電機(jī)之間的速度同步誤差，減小差速震蕩的可能性。

目前，關(guān)于雙電機(jī)系統(tǒng)轉(zhuǎn)速同步控制主要集中在控制結(jié)構(gòu)與控制策略兩個(gè)部分。關(guān)于控制結(jié)構(gòu)的研究主要有并行控制方式[5]、主從控制方式[6]、交叉耦合控制方式[7]，如圖7所示 （其中，ωref為給定角速度；ω1為電機(jī)1 角速度；ω2為電機(jī)2 角速度；K1、K2為兩臺(tái)電機(jī)角速度測(cè)量的系數(shù)；TL1、TL2分別為兩臺(tái)電機(jī)的負(fù)載轉(zhuǎn)矩）。

圖7 雙電機(jī)系統(tǒng)轉(zhuǎn)速同步控制方式Fig.7 Synchronous control modes of dual-motor speed

控制策略方面，由于PI 控制具有結(jié)構(gòu)簡(jiǎn)單、性能穩(wěn)定的特性，在工業(yè)中的各個(gè)領(lǐng)域得到了廣泛的應(yīng)用。隨著電子信息技術(shù)的逐漸發(fā)展，先進(jìn)的控制算法逐步應(yīng)用于雙電機(jī)同步控制策略，如自適應(yīng)控制[8]、神經(jīng)網(wǎng)絡(luò)[9]、非線性高增益補(bǔ)償器[10]和模糊控制[11]等，但先進(jìn)的控制算法往往依賴于控制對(duì)象模型的精度，在實(shí)際系統(tǒng)中存在很大的不穩(wěn)定性，影響了控制效果。

基于以上分析，為了實(shí)現(xiàn)消隙控制與雙電機(jī)同步控制之間的平衡轉(zhuǎn)換，采用主從控制結(jié)構(gòu)的同步控制策略，并應(yīng)用PI 控制方法，通過設(shè)計(jì)合理的轉(zhuǎn)換函數(shù)，使消隙控制階段、消隙控制與共同驅(qū)動(dòng)相互轉(zhuǎn)換的過渡階段和共同驅(qū)動(dòng)控制3 個(gè)階段根據(jù)系統(tǒng)實(shí)際工作狀態(tài)以及相應(yīng)的轉(zhuǎn)換函數(shù)相互轉(zhuǎn)換。

5 機(jī)械臂控制系統(tǒng)設(shè)計(jì)

為了提高整個(gè)機(jī)械臂工作時(shí)的實(shí)時(shí)性，在七自由度機(jī)械臂的控制系統(tǒng)設(shè)計(jì)中，采用了基于ROS 和EtherCAT總線的工業(yè)機(jī)器人控制系統(tǒng)，應(yīng)用于Preempt_rt 實(shí)時(shí)框架下搭建的IgH EtherCAT 主站。

5.1 控制系統(tǒng)硬件總體架構(gòu)

硬件系統(tǒng)通信采用了EtherCAT總線形式，其中腰關(guān)節(jié)、肩關(guān)節(jié)、肘關(guān)節(jié)采用的是前文中所設(shè)計(jì)的雙電機(jī)驅(qū)動(dòng)關(guān)節(jié)，末端的4 個(gè)關(guān)節(jié)采用了新松協(xié)作型機(jī)械臂的單電機(jī)驅(qū)動(dòng)關(guān)節(jié)。

雙電機(jī)關(guān)節(jié)各采用了兩個(gè)EtherCAT 從站分別控制兩個(gè)電機(jī)；末端4 個(gè)關(guān)節(jié)采用了CAN 總線控制方式，通過所設(shè)計(jì)的EtherCAT 和CAN 總線轉(zhuǎn)換器，實(shí)現(xiàn)了末端4 個(gè)關(guān)節(jié)與整機(jī)的通信。工控機(jī)作為主站實(shí)現(xiàn)了整個(gè)機(jī)械臂的控制。硬件連接如圖8所示。

圖8 控制系統(tǒng)硬件整體架構(gòu)Fig.8 Overall hardware architecture of control system

5.2 控制系統(tǒng)軟件整體架構(gòu)

控制系統(tǒng)軟件框架包括ROS和EtherCAT 兩個(gè)部分，其中ROS包括了人機(jī)界面、界面命令處理節(jié)點(diǎn)、MoveIt！、ros_control 4 個(gè)部分，EtherCAT 包括了IgH 主站和EtherCAT從站兩個(gè)部分[12]。

用戶程序在安裝了Preempt_rt實(shí)時(shí)內(nèi)核的Linux 系統(tǒng)下，完成了EtherCAT 主站控制的目標(biāo)，并利用EtherCAT 總線實(shí)現(xiàn)了主站和從站的網(wǎng)絡(luò)通信，從站控制器ESC 的配置、PDI 接口程序和STM32 應(yīng)用程序等構(gòu)成EtherCAT 通訊程序和Cia402應(yīng)用程序組成了STM32 應(yīng)用程序。

軟件架構(gòu)如圖9所示，用戶通過人機(jī)交互界面對(duì)機(jī)器人發(fā)出控制指令，經(jīng)過界面命令處理節(jié)點(diǎn)后，MoveIt！對(duì)笛卡爾坐標(biāo)下的控制指令進(jìn)行運(yùn)動(dòng)學(xué)的規(guī)劃、求解，再使用“Controller manager”對(duì)規(guī)劃好的軌跡進(jìn)行插值計(jì)算，將關(guān)節(jié)坐標(biāo)系下的控制指令傳給控制器進(jìn)行計(jì)算，然后將關(guān)節(jié)角度傳給機(jī)器人硬件抽象節(jié)點(diǎn)，將關(guān)節(jié)角度映射成電機(jī)轉(zhuǎn)角，利用線程通信傳給IgH 用戶程序，再由EtherCAT 總線傳送給ESC 芯片，ESC 芯片將命令傳送給STM32 芯片，最后經(jīng)過CoE 應(yīng)用程序計(jì)算處理后通過硬件輸出控制電機(jī)進(jìn)而控制機(jī)器人。

圖9 控制系統(tǒng)軟件架構(gòu)Fig.9 Software architecture of control system

5.3 IgH EtherCAT 主站構(gòu)建

主站主要負(fù)擔(dān)整個(gè)EtherCAT 網(wǎng)絡(luò)中的控制任務(wù)，在安裝了Preempt_rt實(shí)時(shí)內(nèi)核的Linux 系統(tǒng)上，進(jìn)行IgH主站的設(shè)計(jì)。

用戶程序接口可以將總線配置應(yīng)用到主站，而總線和從站配置完成主站的配置。總線配置包含domain配置和SM 配置；從站配置需要通過應(yīng)用程序告訴IgH 主站總線拓?fù)浣Y(jié)構(gòu)，然后提供供應(yīng)商ID 和產(chǎn)品代碼。

ecrt_master_create_domain() 函數(shù)創(chuàng)建IgH 主站的數(shù)據(jù)域，其返回值為指向數(shù)據(jù)域首地址的指針。將機(jī)械臂的7 個(gè)軸共10 個(gè)伺服電機(jī)使用PDO 對(duì)象在應(yīng)用程序中通過結(jié)構(gòu)體的形式定義好，然后通過PDO 注冊(cè)函數(shù)ecrt_domain_reg_pdo_entry_list()將創(chuàng)建的domain 與上述創(chuàng)建的PDO結(jié)構(gòu)體關(guān)聯(lián)。這樣通過操作數(shù)據(jù)域指針，就可以極為方便地完成所有PDO 數(shù)據(jù)的交換。

SM 的配置包括配置PDO 與傳輸方向。機(jī)械臂的7 個(gè)軸采用一樣的對(duì)象字典，所以7 個(gè)軸的PDO 配置相同。

ecrt_master_slave_config() 函數(shù)實(shí)現(xiàn)從站在總線位置和ID 的配置。將驅(qū)動(dòng)器配置為CSP 模式，采用rt_task_create()配置實(shí)時(shí)任務(wù)名稱和優(yōu)先級(jí)等。

用戶程序初始化與配置完成后，進(jìn)行實(shí)時(shí)性周期任務(wù)程序的設(shè)計(jì)。IgH 主站在從站將數(shù)據(jù)報(bào)文返回給主站后，開始檢查數(shù)據(jù)域和主站狀態(tài)，假如兩項(xiàng)均正常，則與從站配合實(shí)現(xiàn)Cia402 的切換，當(dāng)Cia402 達(dá)到OP 狀態(tài)后，主站讀取從站返回?cái)?shù)據(jù)報(bào)文中的電機(jī)實(shí)際位置和數(shù)字輸入，并將ROS 規(guī)劃好的目標(biāo)位置映射到PDO，再通過LRW 邏輯尋址報(bào)文發(fā)送給從站，完成一個(gè)伺服控制周期，如圖10 所示。

圖10 IgH 主站用戶層軟件流程Fig.10 IgH master user layer software flow

5.4 IgH 伺服從站設(shè)計(jì)

根據(jù)功能類型，可以將從站設(shè)計(jì)分為數(shù)據(jù)通信和應(yīng)用程序兩個(gè)部分。

主站發(fā)送EtherCAT 報(bào)文給LAN 9252 中的ESC，ESC 對(duì)報(bào)文進(jìn)行處理后，將得到的數(shù)據(jù)存放于DPRAM中，并經(jīng)過PDI 接口將數(shù)據(jù)傳送給STM32進(jìn)行報(bào)文處理。若為郵箱數(shù)據(jù)，則直接存入處理器的本地內(nèi)存；若為過程數(shù)據(jù)，則將數(shù)據(jù)賦值給本地內(nèi)存。Cia402 應(yīng)用函數(shù)根據(jù)本地內(nèi)存的數(shù)據(jù)調(diào)用FOC 算法，從而控制伺服電機(jī)，如圖11 所示。

圖11 從站軟件總體結(jié)構(gòu)Fig.11 Overall structure of slave station software

6 試驗(yàn)驗(yàn)證

機(jī)器人的重復(fù)定位精度是驗(yàn)證機(jī)械臂的高精度性能標(biāo)準(zhǔn)之一，根據(jù)GB/T 12642—2013《工業(yè)機(jī)器人性能規(guī)范及其試驗(yàn)方法》中規(guī)定，機(jī)械臂的重復(fù)定位精度為

式中，

式中，、和是對(duì)同一位姿重復(fù)響應(yīng)n次之后所得到的各點(diǎn)集群中心的坐標(biāo)；xj、yj和zj是第j次實(shí)際到達(dá)位置的坐標(biāo)。

如圖12 所示，測(cè)量重復(fù)定位精度采用的是LTD 500 激光跟蹤測(cè)量系統(tǒng)，應(yīng)用前文所述的消隙控制算法，在機(jī)械臂末端施加10 kg 負(fù)載，根據(jù)上述測(cè)試標(biāo)準(zhǔn)在機(jī)械臂的工作空間內(nèi)選取一個(gè)平面內(nèi)的5 個(gè)點(diǎn)測(cè)量其位置坐標(biāo)，重復(fù)測(cè)量30 次。表2給出了各測(cè)試點(diǎn)的機(jī)械臂重復(fù)定位精度。根據(jù)式 （5），由表2可知，該機(jī)械臂最大重復(fù)定位精度誤差（±0.0591 mm）出現(xiàn)在測(cè)試點(diǎn)3，最小重復(fù)定位精度誤差 （±0.0230 mm）出現(xiàn)在測(cè)試點(diǎn)5。由于該機(jī)械臂構(gòu)型以及結(jié)構(gòu)強(qiáng)度設(shè)計(jì)均以七自由度協(xié)作機(jī)器人為模板，而典型七自由度協(xié)作機(jī)器人（Rethink Sawyer、Kuka iiwa）的重復(fù)定位精度為±0.1 mm。因此，本文提出的機(jī)械臂在重復(fù)定位精度指標(biāo)測(cè)試中表現(xiàn)更優(yōu)，同時(shí)也驗(yàn)證了本文所提出的雙電機(jī)共軸驅(qū)動(dòng)消隙算法的有效性。

表2 不同測(cè)量點(diǎn)的機(jī)械臂重復(fù)定位精度Table 2 Repeated positioning accuracy of the robotic arm at different measuring points

圖12 重復(fù)定位精度測(cè)量試驗(yàn)Fig.12 Repeat positioning accuracy measurement experiment

7 結(jié)論

本文基于雙電機(jī)共軸驅(qū)動(dòng)原理，提出并研制出一種新型多電機(jī)驅(qū)動(dòng)的七自由度機(jī)械臂。機(jī)械臂的腰、肩、肘關(guān)節(jié)采用雙電機(jī)共軸驅(qū)動(dòng)，在關(guān)節(jié)上采用消隙及同步控制，可從原理上提高機(jī)械臂的傳動(dòng)精度和承載能力。機(jī)械臂整機(jī)的重復(fù)精度測(cè)量結(jié)果表明，本機(jī)械臂在精度上具有一定的優(yōu)勢(shì)。