摘  要： 為了解決機械臂開環控制精度低的問題，設計了一套基于STM32微控制器的機械臂反饋控制系統。通過QT圖形界面將控制數據輸入上位機，控制數據經串口傳輸到微控制器后驅動機械臂運動；由加速度傳感器和磁通傳感器組成的慣性傳感器節點采集機械臂運動數據傳回微控制器，采用由多個相關的參數可變PID控制器構成的控制器組對機械臂各個部位進行反饋控制。測試結果表明，利用慣性傳感器實現的改進型PID的反饋控制系統比無反饋控制系統精度有較大提高，可用于實現更高精度的機械臂控制。

　　關鍵詞： 機械臂；改進型PID反饋控制；慣性傳感器；嵌入式系統

0 引言

　　機械臂作為一種實用的機器人設備，被廣泛地應用在工業生產和日常生活中。周舟等人[1]控制機械臂進行番茄采摘，根據仿真結果調節控制參數。李鯉[2]以鉆孔機械臂為研究對象，著重對交流伺服電機的驅動進行了研究。劉洋等人[3]在LabVIEW環境下開發了視覺伺服機械臂控制系統的實驗平臺。以上這些系統采用的均是開環控制，雖也能滿足特定的應用場合，但無法滿足高精度機械臂控制的需求。

　　趙杰等人[4]設計了雙關節機械臂控制系統，利用光電編碼器實現了機械臂的閉環控制，但光電編碼器精度仍然較低。因此，本文設計了針對機械臂的改進型PID閉環控制系統，該系統利用慣性傳感器采集機械臂運動數據，在STM32平臺上實現機械臂的反饋控制，旨在提高機械臂的控制精度以滿足精細化動作控制的需求。

1 系統概述

　　系統硬件部分由STM32微控制器、三關節三自由度機械臂和慣性傳感器節點組成。軟件部分包含QT圖形輸入界面、舵機驅動、控制數據生成和反饋控制。系統整體結構如圖1所示，上位機通過QT圖形界面采集用戶輸入的機械臂控制數據后通過串口傳輸到STM32微控制器，微控制器根據控制數據驅動機械臂運動并讀取慣性傳感器節點采集的數據進行反饋控制。

　2 QT圖形輸入界面

　　為了實現良好的用戶體驗和方便遠程控制，本文設計了相應的QT圖形界面。用戶通過上位機可以遠程控制機械臂并監測其運行狀態，STM32微控制器和上位機之間通過RS232串口通信。圖形界面由輸入區和狀態監控區兩部分組成，如圖2所示，用戶可以通過監控區觀察本次動作的完成度以決定是否進行下一個動作。

　　3 反饋數據生成

　　慣性傳感器模塊由加速度傳感器ADXL345和磁通傳感器HMC5883組成，將慣性傳感器模塊綁定在機械臂各個部位，經過數據預處理[5]可分別采集傳感器坐標系下的重力加速度Gs和地磁通量?椎s。記地理坐標系下的重力加速度和地磁通量分別為Ge和e，從傳感器坐標系變換到地理坐標系對應的旋轉矩陣為R，其對應的四元數表示為Q=w+xi+yi+zk。

　　根據梯度四元數按照一定的步長對狀態四元數進行修正，通過反復的迭代融合，求得機械臂各個部位的姿態四元數[6]。將機械臂相鄰兩個部位的姿態四元數分別記為Qn和Qn-1，若其相對姿態四元數為。將四元數轉化為歐拉角即為控制所需的反饋數據。

4 反饋控制

　　為了增強反饋控制的精度，減少抖動的產生，使機械臂在短時間內達到穩定狀態，本系統采用使用簡單、適應性和魯棒性均較強的PID控制器進行反饋控制。但本文中機械臂一共包含三個關節，誤差會隨著機械臂關節延伸逐漸積累。普通的PID控制器只能對手臂各個部位進行獨立控制，不能充分利用機械臂各個部位之間的關系，且控制器參數固定，控制過程所需的時間長。因此，本文在普通PID控制器的基礎上進行了改進，將機械臂各個部位獨立的PID控制器組合起來形成參數可變的PID系統，其控制流程如圖4所示。

　　上臂PID控制器的差分方程為：

　　其中，u（k）為輸出量，e（k）為第k次測量的誤差，Kpu、Kiu和Kdu分別為上臂控制器的比例系數、積分項系數和微分項系數。

　　小臂PID控制器的參數為：

　　其中，Kif和Kdf分別為小臂控制器可變的積分項系數和微分項系數，Kif0和Kdu0為基礎值，λ為比例系數，可通過實際測試進行調節。同理可得手掌處的PID控制器參數。

5 舵機驅動

　　本系統采用MG966R舵機驅動機械臂運動，其扭力大、響應速度快并且可以使機械臂在靜止時仍然保持特定姿態。控制過程中可以通過調節周期為20 ms的PWM波的占空比直接控制機械臂姿態，且占空比與舵機轉角基本成線性關系，避免了通過速度間接控制轉角過程中出現的延遲和精度低的問題。STM32F107微控制器包含PWM波產生模塊，通過配置相應的定時器即可產生特定占空比的PWM波。高級定時器還能產生多路適合電機控制的帶死區互補PWM波，通過單個定時器即可同時驅動多個舵機轉動不同的角度，可以節省出硬件資源以便后期更多功能的開發。

6 測試結果

　　為了驗證利用慣性傳感器實現的PID反饋控制的效果，本文對有無反饋控制兩種情況下的機械臂控制效果分別進行了測試，測試結果如表1所示。其中A組為控制數據，B組為無反饋情況下的控制結果，C組為有反饋情況下的控制結果。從表1可看出，在第一次試驗中有反饋情況下的誤差反而較大，通過分析發現是由于傳感器節點安裝位置離舵機過近而受到舵機磁場干擾造成的。通過改變傳感器的安裝位置，在后期的測試中得到了較為理想的效果。在第二和第三次試驗中，無反饋情況下的平均誤差為5.66°，而有反饋情況下的平均誤差僅為1.5°，控制精度提高了2~7倍，可見改進型的PID反饋控制大大提高了機械臂的控制精度。

7 結論

　　本文設計了一種基于STM32的機械臂反饋控制系統。本系統通過上位機QT圖形界面遠程控制和顯示機械臂運動狀態，實現了友好的人機交互。采用慣性傳感器采集機械臂運動數據，在STM32上實現對機械臂的改進型PID反饋控制，測試結果表明，利用慣性傳感器實現的改進型PID反饋控制較大地提高了機械臂控制的精度，為高精度的機械臂控制提供了一種新的方案。

參考文獻

　　[1] 周舟，王俊.采摘機器人機械臂的控制與聯合仿真[J].電子技術，2013（1）：60-62.

　　[2] 李鯉.基于ARM的機械臂控制系統分析[J].自動化與儀器儀表，2012（2）：176-177.

　　[3] 劉洋，方敏.基于LabVIEW的視覺伺服機械臂控制系統[J].微型機與應用，2012，31（10）：25-27.

　　[4] 趙杰，任思瓂，崔崇信.基于ARM的危險作業機器人機械臂控制系統設計[J].工業儀表與自動化裝置，2012（3）：111-112.

　　[5] DONG W， LIM K Y， GOH Y K， et al. A low-cost motion tracker and its error analysis[C]. IEEE International Conference on Robotics and Automation， Pasadena， 2008： 311-316.

　　[6] SABATINI A M. Quaternion based attitude estimation algorithm applied to signals from body-mounted gyroscopes[J]. Electronics Letters， 2004，40（10）：584-586.