空間直線插補三軸聯動控制系統的設計

　　摘 要：為了進一步改善數控設備中三軸機械手數控系統的性能與可靠性，設計一種基于 STM32 單片機和步進電機的三軸聯動控制控制系統。系統以 STM32F103ZET6 處理器為核心控制器，以步進電機和三軸機械手為執行機構，結合互相獨立的硬件形成統一的整體;系統可根據設定的任務路徑坐標，在三維空間中進行連續空間直線插補，控制三軸機械手末端執行器的運動軌跡，實現精確穩定的三軸聯動控制。實驗結果表明，系統定位精度高，響應速度快，且便于移植，在工業生產中用途廣泛，可有效提高生產效率。

　　關鍵詞：空間直線插補;三軸聯動控制;STM32;三軸機械手;步進電機

　　周佳明; 張宏， 宜賓學院學報 發表時間：2021-11-18

　　計算機數字化控制(CNC)系統利用計算機，通過數字信號控制執行器的開關量和機械量，實現精準控制運動單元的速度、加速度、位置等物理量。肖蘇華[1]采用領域為載體、模型為中心的方法開發計算機數控系統，針對數控領域進行特性分析，提出了一種面向 CNC 系統領域的 CNCVFC 元建模方法。近年來，隨著 SOC 技術的普及，核心控制器向小型化的方向發展，低廉的嵌入式芯片出現井噴現象，嵌入式數控系統[2]得到快速發展，越來越多的學者對嵌入式數控系統的穩定性和可靠性開展研究。張敏[3]分析了嵌入式控制器的重要電路工作原理，設計了以 STM32 為核心控制器的電路原理圖以及 PCB 圖，制作出樣機設備，并做了相關的可靠性試驗，通過對數控系統的現狀分析，確定了一種體積小、功能強大、成本低廉的插補系統的研發方向。目前，典型開放的嵌入式數控系統有 Grbl、Marlin、Sprinter，開發人員只需做好相關的硬件配置，便可實現系統的快速移植，并大大降低研發和生產成本。

　　另一方面，隨著計算機技術和工業自動化的快速發展，三軸機械手[4]在數控機床[5] 、 3D 打印[6] 、醫療器械等高精度領域被廣泛應用。由于機床的加工精度、加工質量和生產效率在很大程度上取決于數控驅動系統的性能和精度，因而要求數控機床的核心控制系統具有穩定的多軸聯動控制性能[7] ，能根據指令信號精確地控制各軸步進電機的運動速度，并能按照任務坐標軌跡在三維空間中精確作業。這些工業生產的需求，對數字控制系統的穩定性和可靠性提出了更高的要求。基于上述背景，以實現穩定的三軸聯動控制和任務軌跡跟蹤為目標，本文設計了一種以 STM32 單片機為核心控制器的三軸聯動運動控制系統，采用步進電機和三軸運動平臺作為執行機構，能夠根據任務坐標連續進行空間直線插補，控制末端執行器的運動軌跡。

　　1 系統結構與方案設計

　　1.1 系統組成

　　系統由 STM32F103ZET6 核心控制器、串口通信模塊、光電隔離模塊、步進電機驅動模塊、按鍵控制、LED 指示模塊和三軸機械手等組成，能夠在空間坐標內實現精確定位，完成任務路徑跟蹤及相關作業。機械手為通用模塊，通過更換機械手的末端執行器和上位機指令可以改變其用途和功能，實現自動分揀、3D 打印、跟蹤焊接[8]等功能。

　　1.2 系統架構設計

　　STM32F103ZET6 核心控制器輸出 PWM 脈沖信號控制步進電機，同時將任務路徑與當前位置進行比較，經空間直線插補算法，輸出進給信號控制三軸機械手末端執行器的運動軌跡。為了便于人工控制，除上位機串口外，通過機械按鍵可控制步進電機的啟動和停止，LED 指示燈指示電機的運行狀態。系統架構如圖 1 所示。

　　1.3 程序流程設計

　　系統的程序流程如圖 2 所示，進入主程序后，先對系統時鐘、步進電機、串口、及相關外設進行初始化，然后進入主循環程序。在循環程序中，重復進行按鍵掃描，當檢測到按鍵 1 被按下，使能步進電機和 PWM 脈沖輸出，并點亮電機狀態指示燈，開啟定時器插補中斷，在插補中斷中判斷當前位置和預定路徑位置的偏差并給定相應脈沖信號，進行空間直線插補，直到執行機構走完任務路徑，失能步進電機，電機狀態指示燈熄滅;當檢測到按鍵 2 按下，再次使能步進電機，使末端執行器回到初始起點位置，為設備的下一次使用做準備。

　　2 硬件平臺設計

　　運動控制系統硬件電路主要包括 STM32 單片機控制模塊、光耦隔離模塊、電機驅動模塊、編碼器模塊、電源模塊、按鍵控制模塊和串口通信模塊。

　　2.1 STM32 核心控制器

　　三軸聯動控制系統采用 STM32F103ZET6 作為運動控制器，它是一高性能 ARM Cotex-M3 內核的 32 位微控制器，主頻 72 MHz，有 144 個引腳、112 個 IO、512 KB flash 和 64 KB SRAM，擁有強大的數字處理功能，具有運行速度快、功耗低的特點。其高級定時器和通用定時器可以輸出精準高頻的 PWM 信號，從而精確控制步進電機。同時，可以實現復雜的軌跡運算以及和外圍設備交互、與上位機及存儲芯片通信[9-10] 。 STM32F103ZET6 最小系統如圖 3 所示。

　　2.2 光耦隔離模塊

　　光電耦合器(簡稱光耦)是一種半導體光電子器件，通過電→光→電的轉換傳輸電信號，一般用于隔離和保護電路。由于具有體積小、壽命長、抗干擾能力強、輸出與輸入隔離和單向傳輸信號等特點，被廣泛應用于數字電路中。本文設計的運動控制系統采用光耦隔離電路對驅動步進電機的脈沖信號輸出進行隔離，達到保護和隔離控制電路的目的[11] 。光電耦合電路如圖 4 所示。

　　2.3 電機驅動模塊

　　2.3.1 步進電機

　　步進電機是一種將電脈沖信號轉換成角位移或直線位移的控制器件(如圖 5)，具有快速啟停的能力。在額定工作條件下，電機轉速和旋轉角位移僅與脈沖信號的頻率和脈沖個數有關。接收到的脈沖數越多，電動機的轉角就越大。脈沖頻率越高，電機轉速越快，但不能超過最高頻率，否則轉矩將迅速降低，電動機停止旋轉[12] 。混合式步進電機兼具反應式和永磁式的優點，具有精度高、步距角小、輸出轉矩大、動態性能好等優點，在高精度控制領域得到廣泛應用[13]。本文選用了三個 57 閉環混合式步進電機作為三軸機械手驅動電機。

　　2.3.2 步進電機驅動器

　　由于本系統要求的精度較高，步進電機的步距角無法滿足系統的使用要求，因此需要使用步進電機驅動器對步進電機的步距角進行細分。本系統使用的步進電機驅動器實物圖及其型號參數如圖 6 所示，它是一種將控制器輸出的脈沖信號轉換成步進電機角位移的執行器件，其輸出的角位移與輸入脈沖數成比例，轉速與脈沖頻率成正比。通過控制脈沖頻率，能夠控制電機轉速，從而實現調速和定位。

　　3 系統設計與規劃

　　3.1 PWM 脈沖與速度設計

　　為了能夠穩定驅動步進電機，需要用使用核心控制器 STM32 單片機輸出一定頻率的 PWM 脈沖，以控制步進電機的啟停、步進方向及其旋轉速度[14-15]。以本系統使用的三軸機械手為例，需要控制三個不同的步進電機。如果要求三個步進電機的速度相同，則只需使用同一個通用定時器或高級定時器的三個不同的輸出通道即可。在本系統中，定時器選用高級定時器 TIM8，PWM 模式選用模式 2，已知 STM32F103ZET6 的高級定時器 TIM8 的時鐘頻率為 72 MHz，設 PWM 周期為T(μs)，則每隔時間T 產生一次插補中斷，那么可以建立等式： ( 1)( 1) 72 psc arr T + + = 其中： psc 為預分頻系數，arr 為計數器重裝載值。

　　由上式可知，PWM 的周期與預分頻系數 psc 和計數器重裝載值arr 成正比，又因為頻率是周期的倒數，則 PWM 的頻率與預分頻系數 psc 和計數器重裝載值arr 成反比，一般來說，可以通過改變計數器重裝載值arr 來控制 PWM 的脈沖頻率，改變頻率就可以改變步進電機旋轉速度;但這個頻率必須小于步進電機的啟動頻率，否則電機無法啟動。

　　3.2 任務軌跡規劃

　　在三軸機器人完成給定的任務之前，需要進行軌跡規劃，軌跡規劃時一般將連續的路徑軌跡進行離散化，用離散點“內插”或“逼近”任務軌跡。當離散的數據點相對分散時，離散點之間的間隔較大，該間隔成為插補盲區，從而產生了實際軌跡和理論軌跡的誤差。在提出的多項式中，對理想笛卡爾軌跡進行了均勻采樣。采樣點的數目被步進電機步距角大小和細分驅動器的精度限制，其精度越高，能夠設置的采樣點就越多[16-17] 。如圖 7 所示，設在笛卡爾坐標系中，空間中的任意一點為 , , Px y z ( e ee)，只要設定一條路徑曲線公式，便可通過采樣獲得多個離散的三維坐標點，再由通訊接口將離散點發送給核心控制器完成相應動作。

　　基于三維直角坐標空間的軌跡規劃比較直觀，通常能直觀地看到末端執行器的運動軌跡，也能達到較高的精度。本文使用 MATLAB 模擬了一條笛卡爾坐標系下的光滑軌跡，如圖 8 所示。

　　根據三軸機械手的運動學模型以及相關參數，可將空間中軌跡的離散坐標點轉換成步進電機的旋轉圈數及旋轉角度，在 STM32 的軟件程序中生成離散的三維位置信息坐標，作為目標終點信號控制各軸上的步進電機運動。

　　4 空間直線插補

　　插補概念源于數值分析數學中的插值，它是一種在已知離散點范圍內構造新數據點的方法。插補指的是數控系統根據電機的特性，用微小折線段來近似替代連續曲線的過程。這種方法在數控系統及相關行業中得到廣泛的應用，是數控機床控制系統的重要組成部分。數控系統中刀具的移動與步進或伺服電機的驅動相關，脈沖通常作為步進或伺服電機的驅動信號來源，因而在數控系統中的插補過程稱為脈沖增量插補，常見脈沖增量插補算法有數字積分法、逐點比較法和Bresenham法等。為了使插補算法更具一般性，通常將曲線軌跡按照特定的規則或算法打斷成若干段連續的微小直線段，而后采用直線插補算法完成插補過程[18-19] 。

　　4.1 逐點比較法

　　逐點比較法[20]是插補算法中應用最廣泛的一種。它既可用作直線插補也可以作圓弧插補，其逼近誤差小于一個脈沖當量，輸出脈沖的速度變化小，輸出脈沖均勻，調節方便且操作直觀，因此它被廣泛地應用于具有多坐標聯動的數控機床[21]。本文采用逐點比較法對第一象限點的線性插補原理進行了分析，再過渡到任意象限的直線插補，分別給出計算過程和計算公式。逐點比較法的直線插補步驟如圖 9 所示。

　　4.2 第一象限內的插補

　　4.2.1 偏差判別

　　偏差判別就是判斷當前加工點和理論加工點之間的相對位置偏差，決定下一步 X 、 Y 軸的運動方向。如圖 10 所示，建立平面直角坐標系，要加工的空間軌跡為線段OE ， OE 是以坐標原點O為起點、以 ( ,) EX Ye e 為終點的平面直線段，點 ( ,) PX Yi i 表示加工點位置。為使問題簡化，設 Xe 、Ye、Ze 均在第一象限，設加工點與OE 的偏差為 Fi ，則： F XY XY i ei ie = − 若 0 Fi = ，表示動點 P 在直線OE 內;若 0 Fi > ，表示動點 P 在直線OE 的上方;若 0 Fi < ，表示動點 P 在直線OE 的下方。

　　4.2.2 坐標進給

　　坐標進給就是根據計算出的偏差，控制指定坐標軸進給一個脈沖，逼近理論值，減小誤差。如圖 11 所示，當 0 Fi = 時，動點 P 在直線內，可向+X 方向進給一步，也可向+Y 方向進給一步，此處規定向+X 方向進給;當 0 Fi > 時，動點+Y 在直線上方，應該向+X 方向進給一步;當 0 Fi < 時，動點 P 在直線下方，應該向+Y 方向進給一步。

　　4.2.3 偏差計算

　　當 0 Fi ≥ 時，加工動點向 +X 方向進給一步，生成一個新的動點 Pi+1 ，設其坐標為 1 1 ( ,) X Y i i + + ，則新動點的偏差值 Fi+1計算公式為： F XY X Y i ei i e + ++ 1 11 = − (1)又因為動點 Pi+1 的坐標可由 P 點表示： 1 1 X X i i + = + (2) Y Y i i +1 = (3)將公式(2)、(3)代入公式(1)中，經化簡可得新的偏差遞推公式： F FY i ie +1 = − (4)同理可得，當 0 Fi < ，加工動點向+Y 方向進給一步后的新偏差值遞推公式為： F FX i ie +1 = + (5)

　　4.2.4 終點判別

　　終點判別就是要判斷是否到達加工終點，如果到達終點則停止插補，如過沒有到達終點則回到第一個步驟，不斷重復整個過程，直到到達軌跡終點。常用的終點判別方法有投影法，終點坐標法和總步長法。本文選取總步長法做插補終點判別。

　　4.3 任意象限的插補

　　由第一象限直線插補公式可推導出其它任意象限的直線插補公式，偏差計算公式基本相同，利用坐標系的絕對值來計算誤差值。任意象限的插補如圖 12 所示，設 L1、L2、 L3、L4 分別表示第 1、2、3、4 象限的直線，則任意象限直線插補的 X 、Y 軸進給方向如表 1 所示。

　　4.4 空間插補原理

　　如圖 13(a)所示，建立空間直角坐標系，設加工軌跡OE 是以坐標原點O為起點，以 ( ,, ) EX Y Z ee e 為終點的空間直線段。設、Ye、Ze 均為正值，將坐標系中的直線OE 分別作兩個二維平面的投影，并以 X 軸為基軸建立兩個平面坐標系 XOY 和 XOZ ，投影到二維平面 XOY 和 XOZ 后如圖 13(b)、13(c)所示，三維空間中的插補就可以分解成二維平面的插補。

　　4.5 空間插補程序流程

　　空間直線插補算法其計算流程[22]如圖 14 所示， Xe 、Ye、Ze 分別表示 X 、Y 、Z 三軸的終點坐標，Fi 、 i S 分別表示投影到 XOY 和 XOZ 平面的偏差，End 等于終點坐標值之和，即表示總步長，用于終點判別。

　　5 實驗與結果

　　(1)實驗操作過程

　　①搭建硬件平臺，對各個模塊進行接線。 ②將上位機編譯的程序寫入 STM32 單片機。 ③打開電源使系統上電，觀察到指示燈點亮。 ④按下按鍵 1，觀察三軸機械手是否按照指定路徑運動。 ⑤按下按鍵 2，觀察三軸機械手是否能夠返回到初始位置。

　　(2)實驗結果

　　按下按鍵 1 后，啟動狀態指示燈點亮，三軸機械手按照任務軌跡平穩運行，到達終點位置后電機停止，狀態指示燈熄滅;按下按鍵 2 后，啟動狀態指示燈再次點亮，三軸機械平穩運行，到達初始位置后停止，狀態指示燈熄滅;整個實驗過程平穩可靠，達到設計的預期要求，實驗結果如圖 15 所示。

　　6 結語

　　本文設計的基于 STM32 單片機和空間直線插補的三軸聯動控制系統能夠實現對三軸機械手進行空間直線插補，能夠精確穩定地控制末端執行器跟蹤任務路徑坐標，實現穩定可靠的三軸聯動控制。該系統硬件電路實現簡單，成本低，可靠性高，軟件程序邏輯清晰且易于移植。實驗結果表明，該系統具備可實現性，在工業生產中用途廣泛，對提高工業生產效率具有重要意義，具有較強的實用價值。