葛連正，趙立軍

（哈爾濱工業(yè)大學(xué) 機(jī)器人技術(shù)與系統(tǒng)國家重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001）

機(jī)器人是一種典型的光機(jī)電一體化系統(tǒng)，已經(jīng)廣泛應(yīng)用于汽車及汽車零部件制造、機(jī)械加工、電子電氣、橡膠及塑料工業(yè)、食品工業(yè)、物流和制造業(yè)等諸多領(lǐng)域中[1-2]。機(jī)器人的軌跡規(guī)劃與控制是機(jī)器人及系統(tǒng)設(shè)計(jì)和應(yīng)用的核心技術(shù)[3]，涉及機(jī)器人的運(yùn)動學(xué)分析、驅(qū)動控制結(jié)構(gòu)、軌跡規(guī)劃算法、軟件設(shè)計(jì)等內(nèi)容，也是機(jī)器人工程專業(yè)人才必須掌握的技能。

我國機(jī)器人專業(yè)的人才培養(yǎng)已開展多年，但大多是由各地方的機(jī)器人應(yīng)用和開發(fā)機(jī)構(gòu)實(shí)施[4]，培養(yǎng)目標(biāo)大多數(shù)是機(jī)器人應(yīng)用人才，缺乏系統(tǒng)的機(jī)器人專業(yè)本科生培養(yǎng)。面向新工科建設(shè)，2019 年教育部設(shè)立了機(jī)器人工程專業(yè)，開始了機(jī)器人專業(yè)的本科生培養(yǎng)。因此，高校需要開發(fā)和完善機(jī)器人的實(shí)驗(yàn)平臺。

基于目前主流的機(jī)器人控制技術(shù)，本文開發(fā)了一種通用的機(jī)器人運(yùn)動規(guī)劃與控制實(shí)驗(yàn)平臺，有助于學(xué)生掌握機(jī)器人的數(shù)學(xué)理論，培養(yǎng)機(jī)器人軌跡規(guī)劃及控制系統(tǒng)的綜合分析、設(shè)計(jì)能力，滿足了機(jī)器人工程專業(yè)學(xué)生的機(jī)器人基礎(chǔ)實(shí)驗(yàn)需求。

1 實(shí)驗(yàn)平臺系統(tǒng)設(shè)計(jì)

1.1 總體框架

機(jī)器人實(shí)驗(yàn)平臺系統(tǒng)主要由倍福CX9020 嵌入式控制器、SANYO 交流伺服電機(jī)、SANYO 伺服驅(qū)動器、24 V 直流電源以及1 套平面五連桿機(jī)構(gòu)組成。其中，機(jī)器人末端夾持一枝中性筆，在雙電機(jī)控制及驅(qū)動下，該中性筆可以實(shí)現(xiàn)在坐標(biāo)平面上的任意軌跡運(yùn)動，而運(yùn)動控制方法及算法可以在嵌入式運(yùn)動控制器里編程實(shí)現(xiàn)?？傮w結(jié)構(gòu)如圖1 所示。

1.2 機(jī)械系統(tǒng)

五連桿機(jī)構(gòu)是多自由度機(jī)構(gòu)中最基本的一種機(jī)構(gòu)，并可以拓展出很多其他形式各異的多自由度機(jī)構(gòu)，因此對五連桿機(jī)構(gòu)的研究具有普遍意義。實(shí)驗(yàn)平臺的機(jī)械系統(tǒng)包括連桿機(jī)構(gòu)、中性筆和框架等部分，主要完成2 自由度平面機(jī)構(gòu)機(jī)器人的模擬運(yùn)動。其中，連桿機(jī)構(gòu)采用“2 驅(qū)動電機(jī)+減速機(jī)”的主、從動桿結(jié)構(gòu)，從而可保證機(jī)器人在實(shí)驗(yàn)平臺的平面軌跡運(yùn)動。該五連桿結(jié)構(gòu)能完成較復(fù)雜的軌跡曲線，已經(jīng)成為現(xiàn)代連桿機(jī)構(gòu)綜合技術(shù)的主要研究方向[5]，對于機(jī)器人專業(yè)人才培養(yǎng)具有普適性。

圖1 實(shí)驗(yàn)系統(tǒng)總體結(jié)構(gòu)圖

1.3 控制系統(tǒng)

實(shí)驗(yàn)平臺的控制系統(tǒng)采用“計(jì)算機(jī)+控制器+驅(qū)動器”的分布式控制結(jié)構(gòu)，這也是目前機(jī)器人控制系統(tǒng)的主流設(shè)計(jì)思路[6]，結(jié)構(gòu)如圖 2 所示。實(shí)驗(yàn)平臺采用基于 EtherCAT 協(xié)議的總線控制結(jié)構(gòu)，使機(jī)器人各控制部件間可以進(jìn)行穩(wěn)定的連接，方便安裝和調(diào)試，提高控制系統(tǒng)的可靠性，并且可以進(jìn)行功能擴(kuò)展。

圖2 機(jī)器人控制系統(tǒng)的硬件結(jié)構(gòu)圖

其中，計(jì)算機(jī)提供交互界面，與控制器通過TCP/IP網(wǎng)絡(luò)通信。倍?？刂破魍ㄟ^ EtherCAT 總線與驅(qū)動器連接，主要進(jìn)行邏輯控制和軌跡規(guī)劃。驅(qū)動器控制伺服電機(jī)實(shí)現(xiàn)機(jī)器人機(jī)械系統(tǒng)的運(yùn)動。伺服電機(jī)安裝17位的光電編碼器，可實(shí)現(xiàn)機(jī)器人關(guān)節(jié)的高精度測量與定位。該控制系統(tǒng)結(jié)構(gòu)簡單、層次清晰，具有功能模塊化和擴(kuò)展性強(qiáng)的特點(diǎn)，方便功能調(diào)試、故障維護(hù)和學(xué)生使用。

2 機(jī)器人運(yùn)動規(guī)劃

2.1 機(jī)器人運(yùn)動學(xué)模型

機(jī)器人運(yùn)動學(xué)是機(jī)器人運(yùn)動規(guī)劃和控制的數(shù)學(xué)基礎(chǔ)，包括機(jī)器人運(yùn)動學(xué)的正解和逆解。該實(shí)驗(yàn)平臺是一個平面二自由度系統(tǒng)，其結(jié)構(gòu)如圖3 所示。其中，點(diǎn)A和E是實(shí)驗(yàn)平臺的2 個驅(qū)動關(guān)節(jié)，點(diǎn)A為系統(tǒng)的坐標(biāo)系原點(diǎn)，點(diǎn)C為中性筆，點(diǎn)B和D是相應(yīng)的中間連接點(diǎn)。a1和a2分別是關(guān)節(jié) 1 和 2 的運(yùn)動角度，l1—l5為各連桿長度。

圖3 機(jī)器人結(jié)構(gòu)簡圖

1）運(yùn)動學(xué)正解。

運(yùn)動學(xué)正解是已知機(jī)器人關(guān)節(jié)角度（a1,a2），求解點(diǎn)C坐標(biāo)（xC,yC）。由圖 3 可知，點(diǎn)A坐標(biāo)為（0, 0），顯然，點(diǎn)B坐標(biāo)為（l1cosa1,l1sina1），點(diǎn)E坐標(biāo)為（l5, 0），點(diǎn)D坐標(biāo)為（l5+l4cosa2,l4sina2），則點(diǎn)C坐標(biāo)滿足：

求解上述方程，可得到機(jī)器人正解：

2）運(yùn)動學(xué)逆解。

運(yùn)動學(xué)逆解是已知點(diǎn)C坐標(biāo)（xC,yC），求解機(jī)器人關(guān)節(jié)角度（a1,a2），機(jī)器人的位置如圖4 所示。

從圖4 中可以看出，機(jī)器人具有4 組逆解，根據(jù)幾何法可得到如下參數(shù)：

這樣，可以得到機(jī)器人的逆解：

圖4 機(jī)器人逆解計(jì)算示意圖

機(jī)器人在逆解的選擇時，可以按照關(guān)節(jié)運(yùn)動路徑最短的原則選?。杭僭O(shè)關(guān)節(jié)1 和2 當(dāng)前角度分別為a10和a20，關(guān)節(jié) 1 的逆解為a11、a12，關(guān)節(jié) 2 的逆解為a21、a22，選取過程如下：

2.2 機(jī)器人軌跡規(guī)劃

實(shí)驗(yàn)平臺的機(jī)器人的軌跡規(guī)劃包括關(guān)節(jié)空間和笛卡爾空間規(guī)劃兩種（見圖5）。前者是給定機(jī)器人的各關(guān)節(jié)的初始和末端角度，基于樣條插補(bǔ)、梯形速度插補(bǔ)等算法進(jìn)行角度運(yùn)動規(guī)劃，不涉及機(jī)器人運(yùn)動學(xué)。后者則基于機(jī)器人的當(dāng)前位姿和末端位姿，確定機(jī)器人插補(bǔ)算法，然后根據(jù)機(jī)器人運(yùn)動學(xué)計(jì)算機(jī)器人各插補(bǔ)點(diǎn)的各軸關(guān)節(jié)角度，最后驅(qū)動機(jī)器人各關(guān)節(jié)運(yùn)動；常用的插補(bǔ)算法包括空間直線、空間圓弧和樣條插補(bǔ)等。

圖5 機(jī)器人軌跡規(guī)劃框圖

實(shí)驗(yàn)平臺通過關(guān)節(jié)點(diǎn)到點(diǎn)、直線、圓弧插補(bǔ)及機(jī)器人運(yùn)動學(xué)對機(jī)器人末端操作器進(jìn)行運(yùn)動軌跡規(guī)劃與控制，而空間復(fù)雜的軌跡都可以通過上述插補(bǔ)算法分段進(jìn)行擬合。

3 機(jī)器人平臺控制軟件

實(shí)驗(yàn)平臺控制器軟件采用TwinCAT 系統(tǒng)，是基于PC 的運(yùn)動控制軟件，功能與傳統(tǒng)的運(yùn)動控制模塊、運(yùn)動控制卡類似，不過它實(shí)現(xiàn)了NC（numerical control）和PLC（programmable logic controller）的無縫集成。TwinCAT 的電機(jī)運(yùn)動控制軸分為三種：PLC 軸、NC軸和物理軸。其中，PLC 程序中定義的軸稱為 PLC軸，在NC 配置界面定義的軸稱為 NC 軸，在 I/O 配置中掃描或添加的運(yùn)動執(zhí)行和位置反饋的硬件稱為物理軸。

PLC 程序?qū)﹄姍C(jī)的控制必須經(jīng)過兩個環(huán)節(jié)：PLC軸到NC 軸，NC 軸再到物理軸。對PLC 軸的控制是指在PLC 中編程，調(diào)用運(yùn)動控制庫的功能塊。對NC軸的控制不需要編程，只需要配置軌跡規(guī)劃、PID 運(yùn)算和 I/O 接口參數(shù)。實(shí)驗(yàn)平臺的控制軟件設(shè)置流程如圖6 所示。

圖6 控制軟件設(shè)置流程圖

4 實(shí)驗(yàn)過程及結(jié)果

4.1 實(shí)驗(yàn)過程

實(shí)驗(yàn)系統(tǒng)如圖7 所示，包括實(shí)驗(yàn)臺、控制柜、實(shí)驗(yàn)操作計(jì)算機(jī)、激光跟蹤儀、靶球和軌跡測試計(jì)算機(jī)等部件。

圖7 實(shí)驗(yàn)平臺及測試系統(tǒng)

該實(shí)驗(yàn)平臺可進(jìn)行機(jī)器人驅(qū)動、運(yùn)動規(guī)劃的理論實(shí)驗(yàn)驗(yàn)證及電機(jī)驅(qū)動和軌跡規(guī)劃控制實(shí)驗(yàn)。這里以直線軌跡規(guī)劃實(shí)驗(yàn)為例，末端起始位置分別設(shè)為（180,173）和（136, 240）mm，插補(bǔ)周期為10 ms，運(yùn)動時間為10 s，實(shí)驗(yàn)平臺的實(shí)驗(yàn)過程如下：

（1）編寫直線軌跡規(guī)劃的 MATLAB 程序，規(guī)劃周期與倍?？刂破鞯牟逖a(bǔ)周期一致，直線軌跡為梯形速度法，仿真結(jié)果如圖8 所示，可以發(fā)現(xiàn)滿足關(guān)節(jié)運(yùn)動的軌跡和速度約束。

圖8 直線軌跡規(guī)劃仿真圖

（2）建立TwinCAT 軟件的機(jī)器人系統(tǒng)電機(jī)模型，包括軸變量、控制和編碼器等參數(shù)。

（3）編寫直線軌跡規(guī)劃的PLC 程序，程序結(jié)構(gòu)如圖9 所示。其中，Main 為主程序，完成程序管理、邏輯控制等；Plan 為規(guī)劃程序。

（4）程序編譯，連接機(jī)器人的2 個NC 軸和物理軸進(jìn)行實(shí)驗(yàn)。

圖9 PLC 程序結(jié)構(gòu)圖

4.2 實(shí)驗(yàn)結(jié)果

為了驗(yàn)證該實(shí)驗(yàn)平臺的規(guī)劃算法和控制系統(tǒng)精度，采用激光跟蹤儀對機(jī)器人末端的運(yùn)動進(jìn)行測量，末端直線軌跡測量結(jié)果如圖10 所示，平面二自由度機(jī)器人運(yùn)動規(guī)劃軌跡精度為0.2 mm，滿足實(shí)驗(yàn)平臺的設(shè)計(jì)要求。

圖10 機(jī)器人直線軌跡測試結(jié)果

同樣，可進(jìn)行機(jī)器人關(guān)節(jié)空間的 3 次多項(xiàng)式、梯形速度插補(bǔ)、圓弧插補(bǔ)及其他復(fù)雜軌跡規(guī)劃算法的實(shí)驗(yàn)。

5 結(jié)語

本文設(shè)計(jì)了一種通用的二自由度機(jī)器人實(shí)驗(yàn)平臺，可以進(jìn)行機(jī)器人關(guān)節(jié)電機(jī)實(shí)驗(yàn)、關(guān)節(jié)空間軌跡規(guī)劃實(shí)驗(yàn)和笛卡爾空間軌跡規(guī)劃等實(shí)驗(yàn)?；谠撈脚_已為本校機(jī)器人工程專業(yè)本科生開設(shè)了機(jī)器人驅(qū)動控制和軌跡控制的2 門實(shí)驗(yàn)課程，取得了良好效果，增強(qiáng)了學(xué)生的機(jī)器人理論基礎(chǔ)，提高了學(xué)生的機(jī)器人機(jī)械設(shè)計(jì)、控制和軟件編程能力。