機器人軌跡規(guī)劃與控制實驗平臺

葛連正，趙立軍

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

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

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

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

1 實驗平臺系統(tǒng)設計

1.1 總體框架

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

1.2 機械系統(tǒng)

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

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

1.3 控制系統(tǒng)

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

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

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

2 機器人運動規(guī)劃

2.1 機器人運動學模型

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

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

1）運動學正解。

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

求解上述方程，可得到機器人正解：

2）運動學逆解。

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

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

這樣，可以得到機器人的逆解：

圖4 機器人逆解計算示意圖

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

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

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

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

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

3 機器人平臺控制軟件

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

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

圖6 控制軟件設置流程圖

4 實驗過程及結(jié)果

4.1 實驗過程

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

圖7 實驗平臺及測試系統(tǒng)

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

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

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

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

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

（4）程序編譯，連接機器人的2 個NC 軸和物理軸進行實驗。

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

4.2 實驗結(jié)果

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

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

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

5 結(jié)語

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