簡介PDF文檔《基于開源倍?？刂葡到y(tǒng)的機器人控制系統(tǒng)設(shè)計》面向機器人控制、運動控制與工業(yè)自動化方向的研發(fā)人員、高校師生及參賽者圍繞如何構(gòu)建高精度超高速開放式機器人控制系統(tǒng)這一工程問題給出了基于倍福Beckhoff開源自動化平臺的設(shè)計方案。壓縮包內(nèi)共1個PDF文件大小約297KB內(nèi)容精煉但覆蓋系統(tǒng)架構(gòu)、硬件選型、軟件平臺、通信協(xié)議與時鐘同步等關(guān)鍵環(huán)節(jié)便于快速建立整體認知。目前已有178人學(xué)習(xí)下載。文檔重點介紹了采用XFC技術(shù)提升系統(tǒng)動態(tài)響應(yīng)性能以TwinCAT為軟件平臺選用ARM9S3C2440作為核心處理器并通過EtherCAT分布式時鐘實現(xiàn)多軸同步有效降低各子流程延遲為機器人多樣化運動控制提供了可參考的工程思路也可作為相關(guān)課題研究和論文寫作的參考文獻。 去年幫客戶搭過一套六軸關(guān)節(jié)機器人的控制系統(tǒng)客戶一開始抱著“能不能全部用開源”的設(shè)想開源運動學(xué)庫、開源示教界面、開源EtherCAT主站。結(jié)果一落地實時性、驅(qū)動兼容性、調(diào)試效率這些現(xiàn)實問題全冒出來了。最后我們折中成一套“開放控制”架構(gòu)以倍福Beckhoff的TwinCAT做實時控制核心外圍把ROS2、PyADS、開源運動學(xué)庫全部接進來做成了一套基于開源生態(tài)和倍福控制系統(tǒng)的機器人控制系統(tǒng)。這套方案穩(wěn)定性和開發(fā)效率都不錯我整理成設(shè)計筆記適合做機器人集成、自動化設(shè)備開發(fā)的工程師參考也適合想從傳統(tǒng)PLC轉(zhuǎn)向開放控制架構(gòu)的研究生和愛好者。整個設(shè)計的核心目標很簡單既能享受開源生態(tài)的算法紅利又能保住工業(yè)控制該有的實時性和可靠性。很多人把“開源”和“商業(yè)”對立起來實際做項目根本不是這么回事。機器人的運動控制鏈路里實時關(guān)節(jié)插補、伺服同步、安全邏輯這一類需要確定性極強的實時系統(tǒng)去扛純開源方案不是做不了但要自己啃的東西太多而路徑規(guī)劃、視覺識別、逆解算法這些上層?xùn)|西開源社區(qū)已經(jīng)很成熟沒必要從零造輪子。倍福TwinCAT加上EtherCAT總線剛好提供了工業(yè)和開放之間的連接點再通過ADS通信協(xié)議把開源生態(tài)掛進來整個系統(tǒng)就活了。1. 方案選型與總體架構(gòu)1.1 為什么把倍福TwinCAT作為控制核心倍福是PC-Based Control路線的代表廠商TwinCAT 3跑在Windows或嵌入式的實時內(nèi)核上本質(zhì)上是一套軟PLC加運動控制平臺。選它做機器人控制核心主要看中三點。第一是開放性。TwinCAT 3支持IEC 61131-3標準語言也能直接用C/C寫算法模塊還能集成MATLAB/Simulink生成的代碼。這意味著開源庫里用C寫的運動學(xué)算法、濾波算法理論上可以封裝成TwinCAT模塊跑在實時任務(wù)里不需要翻譯成梯形圖或者ST這種PLC專用語言。第二是實時性。TwinCAT通過CPU核隔離技術(shù)把Windows的普通任務(wù)和實時任務(wù)分開保證EtherCAT周期在微秒級抖動范圍內(nèi)。機器人控制要求位置環(huán)同步周期一般1ms到4ms倍福在這種場景下的表現(xiàn)很穩(wěn)我實測過多次EtherCAT同步抖動基本能壓到幾十微秒。第三是EtherCAT總線的開放姿態(tài)。EtherCAT協(xié)議本身是開放的倍福作為主站可以帶任意支持CoECANopen over EtherCAT協(xié)議棧的第三方伺服驅(qū)動器。你不需要整套都用倍福的伺服電機完全可以在保證控制精度的前提下選性價比更高的國產(chǎn)或者日系伺服這點對控制成本很重要。1.2 開源組件在系統(tǒng)中扮演的角色這套系統(tǒng)里的“開源”并不是指望TwinCAT本身開源而是用開源組件撐起控制系統(tǒng)中不要求強實時的部分同時用開放的通信接口把它們和TwinCAT打通。我在實際項目中用到的開源組件大概有這幾類ROS2承擔上層智能功能比如視覺識別、機械臂運動規(guī)劃、狀態(tài)機調(diào)度跑在Linux工控機上。開源運動學(xué)庫比如KDL、Trac-IK、Pinocchio用來做正逆運動學(xué)解算、雅可比矩陣計算替代自己推導(dǎo)矩陣的繁瑣工作。PyADS庫Python的ADS通信庫用來和TwinCAT交換數(shù)據(jù)做監(jiān)控、數(shù)據(jù)采集、離線調(diào)參。開源HMI/Web前端用Node-RED或者純Web技術(shù)做示教面板通過ADS把指令寫進PLC比傳統(tǒng)觸摸屏靈活。開源EtherCAT主站比如SOEM、IgH我在實驗室驗證階段會拿它們模擬從站或者做備選方案正式項目里還是以TwinCAT為主。這里面的核心原則是實時鏈路內(nèi)的代碼越簡潔越好能不塞算法進去就不塞非實時的智能決策和數(shù)據(jù)處理全部放開源生態(tài)里。兩層之間用定義好的接口通信。1.3 系統(tǒng)分層架構(gòu)我把整個控制系統(tǒng)分成三層每層職責非常清晰。最上層是智能層Linux系統(tǒng)上跑ROS2做任務(wù)解析、軌跡規(guī)劃、視覺感知。這一層對實時性要求低但對算力、庫生態(tài)要求高適合放開源算法。中間是控制層TwinCAT實時任務(wù)運行在Windows或者倍福嵌入式控制器上負責EtherCAT主站通信、伺服軸同步、邏輯控制、安全保護。這層代碼量不大但每一條都要保證確定性和可靠性。最下層是驅(qū)動層由EtherCAT總線上掛載的伺服驅(qū)動器、IO模塊、安全模塊組成直接驅(qū)動電機和執(zhí)行機構(gòu)。層與層之間控制層通過ADS協(xié)議與智能層通信通過EtherCAT與驅(qū)動層通信。很多項目死就死在層級職責不分比如硬要把動態(tài)避障算法寫進PLC里結(jié)果CPU占用飆高周期抖動變大軸運動就開始不順暢。分層的價值在于每一層都能獨立調(diào)試上層壞了下層還能手動操作讓機器人安全停下來。2. 核心控制邏輯與算法設(shè)計2.1 機器人運動學(xué)建模與正逆解六軸關(guān)節(jié)機器人的運動學(xué)是控制系統(tǒng)的數(shù)學(xué)地基。正解是根據(jù)六個關(guān)節(jié)角度計算末端位姿逆解則是已知末端位姿反推六個關(guān)節(jié)角。正解我一般用D-H參數(shù)法建模每根軸對應(yīng)一個齊次變換矩陣相乘就得到末端位姿。開源庫里KDL和ROS2的urdf都封裝好了這套計算不需要自己寫矩陣乘法的細節(jié)。逆解要復(fù)雜一些。六軸機器人如果滿足后三個關(guān)節(jié)軸線交于一點Pieper準則可以推導(dǎo)解析解速度快、精度高適合實時控制。球腕結(jié)構(gòu)的工業(yè)機器人一般都滿足這個條件。對于不滿足條件的構(gòu)型就得用數(shù)值迭代解比如雅可比迭代或者Trac-IK庫里的優(yōu)化算法但迭代法有收斂性問題靠近奇異位形的時候容易發(fā)散。我的做法是先用開源庫在ROS2環(huán)境里把運動學(xué)驗證好生成一份“關(guān)節(jié)角度查找表”或者逆解函數(shù)然后在TwinCAT里用ST語言實現(xiàn)同樣邏輯保證正逆解結(jié)果一致。如果嫌ST寫起來麻煩也可以把C版逆解封裝成TwinCAT的C模塊這樣實時任務(wù)里直接調(diào)用代碼維護量小很多。2.2 軌跡規(guī)劃與插補軌跡規(guī)劃分為關(guān)節(jié)空間規(guī)劃和笛卡爾空間規(guī)劃。關(guān)節(jié)空間規(guī)劃適合點到點的搬運每個關(guān)節(jié)獨立規(guī)劃速度曲線笛卡爾空間規(guī)劃適合走直線、圓弧這類需要末端保持特定路徑的工藝比如涂膠、焊接。關(guān)節(jié)空間我一般用梯形速度規(guī)劃和S型速度規(guī)劃。梯形簡單但加速度有突變高速運行時機身會抖S型曲線對加速度做平滑處理運動更柔順代價是計算量大一些。用TwinCAT做這類規(guī)劃可以直接使用MC_MoveAbsolute這類運動控制指令它會自動完成加減速處理軸參數(shù)里設(shè)置好速度加速度即可。笛卡爾空間軌跡則麻煩一些需要把末端位姿軌跡離散成插補點每個點逆解成關(guān)節(jié)角再下發(fā)給伺服。這里有兩個選擇一是用TwinCAT的CNC功能通過G代碼描述末端軌跡系統(tǒng)自動插補二是在ROS2中用MoveIt生成軌跡點通過ADS或UDP批量下發(fā)給PLC。前者實時性好適合工藝穩(wěn)定、重復(fù)性高的場景后者算法靈活適合需要動態(tài)避障、視覺引導(dǎo)的場景。2.3 伺服控制三環(huán)與PDO參數(shù)配置伺服驅(qū)動器的控制邏輯分三個環(huán)電流環(huán)、速度環(huán)、位置環(huán)電流環(huán)響應(yīng)最快速度環(huán)次之位置環(huán)最慢。大多數(shù)EtherCAT伺服驅(qū)動器內(nèi)部已經(jīng)實現(xiàn)了電流環(huán)和速度環(huán)位置環(huán)可以由驅(qū)動器自己跑也可以由TwinCAT NC運動控制來做。六軸機器人聯(lián)動要求各軸位置同步精度高我傾向把位置環(huán)放在TwinCAT側(cè)所有軸的插補周期統(tǒng)一同步性更有保障。PDO配置是EtherCAT伺服調(diào)試里最繞不開的環(huán)節(jié)。PDO就是過程數(shù)據(jù)對象決定每個周期主站和驅(qū)動器之間交換哪些數(shù)據(jù)。RPDO接收配置控制字、目標位置、目標速度、運行模式等TxPDO發(fā)送配置狀態(tài)字、實際位置、實際速度、報警代碼等。倍福TwinCAT里配置PDO映射的方法是在I/O設(shè)備下掃描出驅(qū)動器后進入CoE在線界面找到對應(yīng)的PDO映射對象把需要的對象ID填入PDO映射項。常用CoE對象我列在下面這些是必須搞清楚的核心參數(shù)對象索引名稱作用0x6040Controlword控制字控制伺服使能、急停0x6041Statusword狀態(tài)字讀取伺服當前狀態(tài)0x6060Modes of operation運行模式比如位置模式/速度模式0x607ATarget position目標位置位置模式下寫入0x6080Max motor speed電機最大轉(zhuǎn)速單位與驅(qū)動器定義有關(guān)0x6091Gear ratio電子齒輪比用于電機與機械負載的換算0x607DSoft position limit軟件限位防止機構(gòu)撞機0x606CVelocity actual value實際速度反饋PDO映射規(guī)則因驅(qū)動器品牌而異但思路一致先確認驅(qū)動器支持的PDO長度再按字節(jié)對齊原則組合對象最后把參數(shù)下載到驅(qū)動器EEPROM里。2.4 TwinCAT與開源生態(tài)的融合方式TwinCAT和外部程序通信的官方通道是ADSAutomation Device Specification。只要在TwinCAT路由里配置好AMSNetId和端口號外部程序就能用ADS讀寫PLC變量像敲鍵盤一樣方便。具體融合方式有四種。第一種是外部直接讀寫符號變量適合監(jiān)控和調(diào)試第二種是調(diào)用TwinCAT的.NET或C接口把開源算法打包成動態(tài)庫供PLC調(diào)用第三種是把算法寫在C模塊里直接嵌入TwinCAT實時任務(wù)第四種是通過UDP/TCP走自定義協(xié)議把ROS2和TwinCAT連起來。我最常用的是第一種和第四種。第一種用于日常調(diào)試第四種用于正式的控制流。羅斯2那邊把規(guī)劃好的關(guān)節(jié)角打包成JSON通過UDP發(fā)給TwinCAT的通信功能塊PLC負責解析并執(zhí)行插補。這樣做的好處是兩邊徹底解耦ROS2崩了PLC還能讓機器人安全停在當前位置。3. 實操過程從零搭建一套六軸機器人控制系統(tǒng)3.1 硬件準備與選型硬件準備這部分控制器我推薦倍福的CX系列嵌入式控制器比如CX5120體積小、功耗低適合裝在機器人電控柜里算力足夠跑TwinCAT實時任務(wù)。如果后續(xù)要跑視覺或者更復(fù)雜的算法可以用C6640這種高性能工業(yè)PC把TwinCAT和Linux虛擬化跑在一起不過這個方案復(fù)雜度也上去了。伺服驅(qū)動器建議選支持CoE的通用EtherCAT伺服我用過倍福AX5000也用過其他品牌的驅(qū)動器都能通過PDO配置接入。電機按關(guān)節(jié)減速比和額定扭矩選型最好帶絕對值編碼器這樣上電不需要回零用戶體驗差很多。IO模塊用EL系列的輸入輸出端子即可安全回路最好配專用的安全端子或者TwinSAFE方案。3.2 TwinCAT實時環(huán)境配置環(huán)境搭建這個環(huán)節(jié)看似簡單實際坑最多。裝好TwinCAT 3 XAE后首先要做的是把EtherCAT網(wǎng)卡的驅(qū)動換成TwinCAT專用驅(qū)動。在TwinCAT XAE的“Real-Time Ethernet”設(shè)置里找到你準備用于EtherCAT的網(wǎng)卡把綁定關(guān)系切到TwinCAT驅(qū)動這一步做錯的話系統(tǒng)根本掃不到設(shè)備。然后是實時核的配置。打開“Real-Time”設(shè)置把CPU的核分配給TwinCAT實時任務(wù)建議至少留出一個獨立核給實時任務(wù)使用Windows和其他程序跑在剩余核上。BIOS里還要關(guān)掉CPU的C-State節(jié)能和動態(tài)頻率調(diào)整否則實時任務(wù)周期會出現(xiàn)明顯抖動。完成這些后用TwinCAT的實時監(jiān)控窗口測一下抖動值如果能穩(wěn)定在幾十微秒就說明環(huán)境合格了。3.3 NC軸與伺服驅(qū)動器PDO配置實操在TwinCAT里創(chuàng)建軸和PDO映射我總結(jié)了幾個標準動作。先把EtherCAT主站設(shè)備添加好在I/O設(shè)備里掃描實際硬件。掃描到伺服驅(qū)動器后雙擊進入“Process Data”配置頁。以常見的CIA402對象為例RPDO需要映射至少四個對象控制字0x6040、目標位置0x607A、目標速度0x60FF、運行模式0x6060對應(yīng)的PDO項是0x1600到0x1603。TxPDO映射對應(yīng)的0x1A00到0x1A03包含狀態(tài)字0x6041、實際位置0x6064、實際速度0x606C、跟隨誤差等字段。配置無誤后在“CoE Online”里把PDO配置寫入驅(qū)動器EEPROM斷電保存。如果PDO映射里的對象長度和驅(qū)動器內(nèi)部定義不一致設(shè)備狀態(tài)會在OP和SAFEOP之間反復(fù)跳動這個問題后面在故障排查部分細講。軸參數(shù)配置要去NC界面新建一個軸對象并將其鏈接到對應(yīng)的EtherCAT驅(qū)動器。NC軸里要設(shè)置用戶單位換算這個換算直接決定位置指令準不準。以我常用的減速比100:1的關(guān)節(jié)為例電機編碼器每圈131072個增量輸出軸旋轉(zhuǎn)一圈需要電機轉(zhuǎn)100圈用戶單位用“度”那么一個用戶單位1°對應(yīng)的編碼器增量個數(shù)是 131072×100÷360約等于36408.89。在NC軸配置里把這個比例關(guān)系填進去PLC里的軸位置值就變成了直觀的度數(shù)而不是一堆脈沖數(shù)。3.4 用Python和PyADS實現(xiàn)上位機監(jiān)控PyADS是開源社區(qū)里非常成熟的ADS通信庫安裝簡單能直接讀寫TwinCAT里的符號變量。我通常會寫一個小的Python監(jiān)控腳本用來讀取機器人各軸的實際位置、速度、報警信息數(shù)據(jù)可以存到本地或者推送到Web界面。import pyads # 連接TwinCAT路由里的PLC plc pyads.Connection(192.168.1.10.1.1, 851) plc.open() # 讀取NC軸的實際位置符號名和TwinCAT里一致 pos_axis1 plc.read_by_name(MAIN.nActPosAxis1, pyads.PLCTYPE_LREAL) print(軸1實際位置: {:.3f} deg.format(pos_axis1)) # 寫入目標速度 plc.write_by_name(MAIN.nSetVelAxis1, 15.0, pyads.PLCTYPE_LREAL) plc.close()這類腳本最大的價值是調(diào)試效率提升。以前調(diào)機器人的時候改一個位置參數(shù)要反復(fù)在觸摸屏上點現(xiàn)在直接寫個小腳本批量下發(fā)、批量記錄幾輪對比實驗下來省非常多時間。不過要注意PyADS跑在Windows或Linux的非實時側(cè)只能用于監(jiān)控和非實時的參數(shù)下發(fā)不能拿來寫實時插補邏輯。3.5 在ROS2中做一次軌跡規(guī)劃下發(fā)ROS2這塊我常用的組合是URDF描述機器人模型MoveIt做運動規(guī)劃再配合Trac-IK做逆解。先在ROS2里啟動機器人描述和運動規(guī)劃節(jié)點用MoveIt的Python接口給一個目標末端位姿規(guī)劃結(jié)果是一系列帶時間戳的關(guān)節(jié)角度軌跡點。把軌跡點通過UDP或者ROS2的topic橋接傳給TwinCAT側(cè)。TwinCAT里寫一個通信功能塊接收軌跡點并放入緩沖區(qū)然后按固定周期讀取緩沖區(qū)的下一組關(guān)節(jié)角度經(jīng)過平滑處理后發(fā)給NC軸執(zhí)行。這里要特別注意時間戳同步問題ROS2規(guī)劃的軌跡頻率通?？刂频煤芨叩掳l(fā)的數(shù)據(jù)如果延遲太大PLC側(cè)就要自己保證平滑過渡不能讓機器人直接跳變到下一個點。4. 常見問題與排查技巧實錄4.1 故障排查速查表做機器人控制系統(tǒng)不會一帆風(fēng)順我直接把最常踩的坑整理成一張表現(xiàn)場調(diào)試時可以先照著排查。問題現(xiàn)象可能原因排查方法EtherCAT從站狀態(tài)在OP和SAFEOP間反復(fù)切換PDO映射長度配置錯誤或者從站廠商代碼不兼容檢查過程數(shù)據(jù)映射核對每個對象的數(shù)據(jù)類型和位數(shù)軸使能后立即報警無法運動驅(qū)動器STO安全回路沒有閉合或限位信號觸發(fā)檢查安全輸入端子狀態(tài)確認急?；芈窂?fù)位軸實際位置和期望位置偏差很大單位換算比例錯誤或者編碼器方向反了用低速點動測試對比NC指令位置和驅(qū)動反饋位置上位機ADS連接超時TwinCAT路由沒有添加客戶端或者防火墻攔截檢查TwinCAT路由配置添加客戶端AMSNetId并放行端口運動過程中末端軌跡有抖動插補周期不穩(wěn)定或者濾波參數(shù)不合適查看實時抖動監(jiān)控調(diào)整伺服增益和濾波時間常數(shù)4.2 實時性與抖動排查EtherCAT系統(tǒng)的實時抖動直接表現(xiàn)為軸運動不流暢、軌跡精度差甚至偶爾報警。我在TwinCAT“Real-Time”窗口里看過幾種典型情況抖動在50微秒以內(nèi)屬于正常100微秒左右還能接受如果超過500微秒就要檢查環(huán)境了。最容易引發(fā)抖動的是Windows系統(tǒng)層面。電源計劃要切到高性能模式CPU的Turbo Boost如果會影響實時性也可以關(guān)掉。分給TwinCAT的獨立CPU核不能有任何其他后臺任務(wù)搶占包括殺毒軟件、Windows Update這些都得在工控機上禁用。還有一個容易忽略的點如果你的EtherCAT主站和上位機通信用的是同一塊網(wǎng)卡通信量大會上肯定影響實時性最好分開兩塊物理網(wǎng)卡。4.3 運動學(xué)與軌跡問題運動學(xué)相關(guān)的問題一般出現(xiàn)在兩個場景逆解選不出解以及笛卡爾規(guī)劃軌跡經(jīng)過奇異點時出問題。逆解選解要看關(guān)節(jié)限位和工作空間的匹配同一個末端位姿可能有多個逆解控制程序要設(shè)定優(yōu)先級規(guī)則比如按“軸運動累計最小”的原則選解。軌跡經(jīng)過奇異點時末端速度會突然變得很大關(guān)節(jié)角速度可能逼近極限甚至飛車。我在ROS2里用Trac-IK做規(guī)劃時會提前檢查雅可比矩陣的條件數(shù)發(fā)現(xiàn)接近奇異就重新規(guī)劃路徑繞開奇異區(qū)域。在TwinCAT側(cè)執(zhí)行時也要給每個軸設(shè)置軟件限位和速度限幅作為最后一道安全防線。4.4 開源組件與TwinCAT版本兼容問題開源組件版本迭代快和TwinCAT組合起來偶爾會有兼容問題。PyADS新版本如果和系統(tǒng)中的TwinCAT版本差了比較多可能出現(xiàn)變量類型解析錯誤比如PLC的LREAL是64位浮點數(shù)但你在Python里用PLCTYPE_REAL32位去讀讀出來的值就會莫名其妙地亂跳。凡是涉及浮點數(shù)據(jù)讀寫一定要確認好類型長度。ROS2版本和Linux系統(tǒng)的搭配也是個經(jīng)典問題。ROS2的發(fā)行版強依賴Ubuntu版本裝的時候最好按官方支持矩陣來不要自己去編源碼硬適配。集成開源庫時還要注意許可證比如某些GPL協(xié)議的庫如果動態(tài)鏈接可以規(guī)避傳染性但如果直接改源碼進你的商業(yè)代碼后續(xù)發(fā)布會有合規(guī)風(fēng)險這些在項目啟動階段就要想清楚。5. 寫在最后一點設(shè)計心得做完這套系統(tǒng)我最深的體會是控制系統(tǒng)選型的核心不在“要不要用開源”而在搞清楚哪些環(huán)節(jié)必須實時、哪些環(huán)節(jié)可以開放。把實時的活交給TwinCAT這種專業(yè)平臺把智能的活交給開源生態(tài)EtherCAT當骨骼ADS當血管兩邊各司其職系統(tǒng)才穩(wěn)定又靈活。最后再分享一個小技巧開發(fā)階段別急著接真實機器人先用TwinCAT的仿真軸或者離線模型把邏輯跑通再用開源EtherCAT主站做一個虛擬從站做通信測試整套系統(tǒng)的風(fēng)險會大幅降低。等仿真全部通過再接伺服和機械本體調(diào)試時間能縮短一半以上。這個思路不僅適用于機器人控制凡是要做多軸運動控制和開源算法融合的項目都可以照這個框架去搭。本文還有配套的精品資源點擊獲取