機器人軌跡規劃與控制實驗平臺

葛連正，趙立軍

（哈爾濱工業大學 機器人技術與系統國家重點實驗室，黑龍江 哈爾濱 150001）

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

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

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

1 實驗平臺系統設計

1.1 總體框架

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

1.2 機械系統

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

圖1 實驗系統總體結構圖

1.3 控制系統

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

圖2 機器人控制系統的硬件結構圖

其中，計算機提供交互界面，與控制器通過TCP/IP網絡通信。倍福控制器通過 EtherCAT 總線與驅動器連接，主要進行邏輯控制和軌跡規劃。驅動器控制伺服電機實現機器人機械系統的運動。伺服電機安裝17位的光電編碼器，可實現機器人關節的高精度測量與定位。該控制系統結構簡單、層次清晰，具有功能模塊化和擴展性強的特點，方便功能調試、故障維護和學生使用。

2 機器人運動規劃

2.1 機器人運動學模型

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

圖3 機器人結構簡圖

1）運動學正解。

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

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

2）運動學逆解。

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

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

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

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

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

2.2 機器人軌跡規劃

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

圖5 機器人軌跡規劃框圖

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

3 機器人平臺控制軟件

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

PLC 程序對電機的控制必須經過兩個環節：PLC軸到NC 軸，NC 軸再到物理軸。對PLC 軸的控制是指在PLC 中編程，調用運動控制庫的功能塊。對NC軸的控制不需要編程，只需要配置軌跡規劃、PID 運算和 I/O 接口參數。實驗平臺的控制軟件設置流程如圖6 所示。

圖6 控制軟件設置流程圖

4 實驗過程及結果

4.1 實驗過程

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

圖7 實驗平臺及測試系統

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

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

圖8 直線軌跡規劃仿真圖

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

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

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

圖9 PLC 程序結構圖

4.2 實驗結果

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

圖10 機器人直線軌跡測試結果

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

5 結語

本文設計了一種通用的二自由度機器人實驗平臺，可以進行機器人關節電機實驗、關節空間軌跡規劃實驗和笛卡爾空間軌跡規劃等實驗?；谠撈脚_已為本校機器人工程專業本科生開設了機器人驅動控制和軌跡控制的2 門實驗課程，取得了良好效果，增強了學生的機器人理論基礎，提高了學生的機器人機械設計、控制和軟件編程能力。