面對(duì)一臺(tái)ZMC300E運(yùn)動(dòng)控制器，我們?cè)撛鯓幼屗ぷ髌饋?lái)？本文帶你了解其核心算法原理，并詳細(xì)講解如何操縱龍門架畫筆平臺(tái)繪圖。

?前言

ZMC300E（點(diǎn)擊了解詳情）是廣州致遠(yuǎn)電子股份有限公司開(kāi)發(fā)的最新一代智能總線型 EtherCAT 主站控制器，使用A8內(nèi)核的AM335X為處理器核心，配合Xenomai高實(shí)時(shí)性Linux系統(tǒng)，支持作為EtherCAT主站使用，其通訊可實(shí)現(xiàn)ms周期任務(wù)抖動(dòng)±15μs以內(nèi)，可滿足運(yùn)動(dòng)控制器的高實(shí)時(shí)性要求。

圖1ZMC300E控制器

可以看到，運(yùn)動(dòng)控制器的強(qiáng)項(xiàng)是EtherCAT實(shí)時(shí)通信，主要工作就是發(fā)號(hào)施令，通過(guò)網(wǎng)絡(luò)命令多個(gè)電驅(qū)和電機(jī)工作。通俗的說(shuō)，運(yùn)動(dòng)控制器干腦力活，動(dòng)口不動(dòng)手，電驅(qū)和電機(jī)干體力活，專干重活累活。

那么，運(yùn)動(dòng)控制器憑什么當(dāng)上智力擔(dān)當(dāng)?shù)模吭瓉?lái)運(yùn)動(dòng)控制器是計(jì)算能手，擁有三大絕活算法：軌跡插補(bǔ)，速度規(guī)劃，運(yùn)動(dòng)前瞻。憑借這三大絕活，運(yùn)動(dòng)控制器就能指揮電驅(qū)和電機(jī)完成各種運(yùn)動(dòng)任務(wù)。

?算法原理

接下來(lái)簡(jiǎn)單地介紹運(yùn)動(dòng)控制器的三大絕活算法：軌跡插補(bǔ)，速度規(guī)劃，運(yùn)動(dòng)前瞻。這三大算法包含較多子類別的算法，本文只對(duì)最基礎(chǔ)最實(shí)用的子算法進(jìn)行介紹，后面實(shí)戰(zhàn)繪圖也會(huì)用到這些算法：軌跡插補(bǔ)-直線插補(bǔ)，速度規(guī)劃-梯形速度規(guī)劃，運(yùn)動(dòng)前瞻-拐角減速。1.軌跡插補(bǔ)運(yùn)動(dòng)任務(wù)的核心任務(wù)就是繪制各種軌跡，運(yùn)動(dòng)控制器的看家本領(lǐng)就是軌跡插補(bǔ)。插補(bǔ)算法根據(jù)給定的信息進(jìn)行數(shù)字計(jì)算，不斷計(jì)算出參與運(yùn)動(dòng)的各坐標(biāo)軸的進(jìn)給指令，然后分別驅(qū)動(dòng)各自相應(yīng)的執(zhí)行部件產(chǎn)生協(xié)調(diào)運(yùn)動(dòng)，以使被控機(jī)械部件按理想的路線與速度移動(dòng)。插補(bǔ)最常見(jiàn)的兩種方式是直線插補(bǔ)和圓弧插補(bǔ)。插補(bǔ)運(yùn)動(dòng)至少需要兩個(gè)軸參與，進(jìn)行插補(bǔ)運(yùn)動(dòng)時(shí)，首先需要建立坐標(biāo)系，將規(guī)劃軸映射到相應(yīng)的坐標(biāo)系中，運(yùn)動(dòng)控制器根據(jù)坐標(biāo)映射關(guān)系，控制各軸運(yùn)動(dòng)，實(shí)現(xiàn)要求的運(yùn)動(dòng)軌跡。直線運(yùn)動(dòng)的插補(bǔ)示意圖如下，插補(bǔ)運(yùn)動(dòng)中由起始點(diǎn)處沿 X 方向走一小段（給一個(gè)脈沖當(dāng)量軸走一段固定距離），發(fā)現(xiàn)終點(diǎn)在實(shí)際輪廓的下方，則下一條線段沿 Y 方向走一小段，此時(shí)如果線段終點(diǎn)還在實(shí)際輪廓下方，則繼續(xù)沿 Y 方向走一小段，直到在實(shí)際輪廓上方以后，再向 X 方向走一小段，依次循環(huán)類推，直到到達(dá)輪廓終點(diǎn)為止。

圖2直線插補(bǔ)示意圖除了直線插補(bǔ)，ZMC300E還支持多種軌跡插補(bǔ)：圓弧插補(bǔ)，橢圓弧插補(bǔ)，漸開(kāi)線插補(bǔ)，螺旋線插補(bǔ)，貝塞爾曲線插補(bǔ)。2.速度規(guī)劃運(yùn)動(dòng)控制器還可以為軌跡插補(bǔ)賦予速度規(guī)劃。速度規(guī)劃這里特指，在已知運(yùn)動(dòng)的起點(diǎn)和終點(diǎn)和完整路徑的情況下，基于給定的時(shí)間約束, 最大速度約束和最大加速度減速度約束的條件，設(shè)計(jì)運(yùn)動(dòng)速度隨時(shí)間的曲線。常見(jiàn)的速度規(guī)劃算法有梯形速度規(guī)劃算法，S形速度規(guī)劃算法。梯形速度曲線，也稱為直線加減速或T型加減速。其算法十分簡(jiǎn)易，規(guī)劃周期耗時(shí)短，有利于縮減系統(tǒng)的連續(xù)運(yùn)行時(shí)間，從而提高系統(tǒng)的運(yùn)動(dòng)控制速度，實(shí)施起來(lái)比較容易，應(yīng)用廣泛。

圖3梯形速度規(guī)劃-速度示意圖

該速度由3段組成，第一個(gè)段黃色段是加速段，第二段（綠色段）是勻速段，第三段（黃色段）是減速段。

圖4梯形速度規(guī)劃-位移示意圖圖5梯形速度規(guī)劃-加速度示意圖

除了梯形速度規(guī)劃，ZMC300E還支持多種S型速度規(guī)劃：加加速度（二次加速度）給定條件的S型速度規(guī)劃，彎曲時(shí)間給定條件的S型速度規(guī)劃，和加減速時(shí)間和彎曲時(shí)間給定條件的S型速度規(guī)劃。

3.運(yùn)動(dòng)前瞻

運(yùn)動(dòng)控制器能夠更加高效規(guī)劃運(yùn)動(dòng)，這要求運(yùn)動(dòng)控制器支持連續(xù)插補(bǔ)運(yùn)動(dòng)，支持運(yùn)動(dòng)前瞻。

這里給一個(gè)矩形繪圖作為連續(xù)插補(bǔ)的示例。一個(gè)普通的矩形軌跡如下圖。

圖6矩形軌跡該矩形含有四段軌跡。

使用梯形速度規(guī)劃，可以得到以下的速度曲線，可以看到，由4段曲線組成。

圖7不開(kāi)啟運(yùn)動(dòng)前瞻的速度-時(shí)間示意圖不開(kāi)啟連續(xù)插補(bǔ)和運(yùn)動(dòng)前瞻，每段速度都減速到零。縱軸為速度，橫軸為時(shí)間。下圖是開(kāi)啟連續(xù)插補(bǔ)的速度-時(shí)間圖。速度是通過(guò)位置和發(fā)射時(shí)間推算出來(lái)的，不完全精確，速度出現(xiàn)毛刺是正常的。

圖8連續(xù)插補(bǔ)速度-時(shí)間示意圖

開(kāi)啟連續(xù)插補(bǔ)，每段速度不再減速到0。可以看到，時(shí)間相比不開(kāi)啟運(yùn)動(dòng)前瞻更短。但是有個(gè)嚴(yán)重問(wèn)題：拐角處的速度是最大值，過(guò)高的速度可能影響電機(jī)和刀具穩(wěn)定性。

圖9運(yùn)動(dòng)前瞻速度-時(shí)間示意圖

開(kāi)啟連續(xù)插補(bǔ)和運(yùn)動(dòng)前瞻，拐角減速，每段速度不再減速到0，而是根據(jù)拐角角度決定速度。可以看到，時(shí)間相比不開(kāi)啟運(yùn)動(dòng)前瞻更短，相比連續(xù)插補(bǔ)更長(zhǎng)，避免刀具高速通過(guò)拐角處的問(wèn)題。

運(yùn)動(dòng)控制器的運(yùn)動(dòng)前瞻功能可以根據(jù)用戶的運(yùn)動(dòng)路徑自動(dòng)計(jì)算出平滑的速度規(guī)劃，以防止軌跡的拐角處速度過(guò)快。運(yùn)動(dòng)前瞻包括三種模式：拐角減速、倒角、小圓限速，而這三種模式也可混合使用。拐角減速會(huì)根據(jù)夾角大小確定拐角處的最大速度，倒角則會(huì)設(shè)置前后兩個(gè)軌跡在拐角處的倒角，而小圓限速則會(huì)對(duì)半徑小于設(shè)置值的圓弧軌跡進(jìn)行限速。

?實(shí)戰(zhàn)繪圖

介紹完了算法原理，接下來(lái)我們使用ZMC300E運(yùn)動(dòng)控制器來(lái)進(jìn)行實(shí)戰(zhàn)繪圖。

首先把運(yùn)動(dòng)控制器和畫筆平臺(tái)使用網(wǎng)線連接。畫筆平臺(tái)是一個(gè)3軸的龍門架，可以在xyz三軸空間移動(dòng)，夾具上只有一根常見(jiàn)的鉛筆。

圖10畫筆平臺(tái)

選擇一幅SVG格式圖片作為我們畫圖的圖案，這里我們選擇的是致遠(yuǎn)電子公司的logo圖片。

圖11Logo圖片

對(duì)于輸入圖片，我們可以把矢量圖解析成多個(gè)筆畫，每個(gè)筆畫解析成直線曲折相連，這樣就可以得到對(duì)應(yīng)繪圖軌跡。G代碼（G-code）是最為廣泛使用的計(jì)算機(jī)數(shù)控編程語(yǔ)言，主要在計(jì)算機(jī)輔助制造中用于控制自動(dòng)機(jī)床。

我們使用開(kāi)源工具svg_to_gcode，把svg文件轉(zhuǎn)成G代碼文件。

圖12G代碼文件

查看G代碼文件，可以看到都是G1指令，說(shuō)明都是直線插補(bǔ)命令。這說(shuō)明我們只用直線插補(bǔ)命令就能完成繪圖。對(duì)應(yīng)的繪圖圖案如下，不同的顏色對(duì)應(yīng)不同的筆劃，可以看到圖片包含十八筆的仿真筆畫圖。

圖13 筆畫軌跡圖片

每一筆劃都要有進(jìn)筆（筆尖觸及紙張），作畫（筆尖寫字）和退筆（筆尖遠(yuǎn)離紙張）。G代碼中進(jìn)筆對(duì)應(yīng)M3指令（進(jìn)刀），退筆對(duì)應(yīng)M5指令（退刀）。這樣才能書寫多個(gè)筆劃。

目標(biāo)圖形總共有十八筆的筆畫，要想作畫就得先對(duì)筆畫進(jìn)行排序。這里使用最直觀的筆劃排序算法，先左后右，先上后下。
加入筆畫的連接，可以得到以下的筆畫仿真繪圖。

圖14 連筆筆畫繪圖

可以看到，仿真效果大體上符合我們的預(yù)期。我們就可以在運(yùn)動(dòng)控制器中運(yùn)行G代碼腳本。使用ssh連接運(yùn)動(dòng)控制器，啟動(dòng)控制程序。設(shè)置左下方為原點(diǎn)，沿著紙張往右邊寫字，紙張寬度至少280mm，高度60mm。使用梯形速度規(guī)劃，配置最大速度100mm/s，最大加速度和最大減速度為100mm/s^2。進(jìn)筆和退筆，對(duì)應(yīng)z軸上下的運(yùn)動(dòng)，進(jìn)筆量和退筆量都設(shè)為10mm。為了提高繪圖效率，開(kāi)啟了連續(xù)插補(bǔ)，運(yùn)動(dòng)前瞻和拐角減速。

圖15 繪圖可以看到，我們使用ZMC300E控制器漂亮地完成了繪圖。