基于單片機的六自由度機械手臂控制系統設計

陳心怡 張春雨 朱麗華

（1.池州職業技術學院，安徽 池州 247000;2.安徽科技學院，安徽 滁州 239000）

一、引言

機械臂也稱工業機器人，是以運動作為控制對象的智能控制裝備，主要由手臂、舵機、抓手等組成[1]。機器人手臂的張開、夾緊等系列動作由電動機驅動，并準確地反饋到可編程邏輯控制器。在工業生產中控制機械臂完成我們所需要的夾取和分揀動作，本項目便是圍繞六自由度機械臂控制系統展開的。通常一個52系列的單片機包含有FLASH ROM、RAM、3個16位的定時器/計數器和1個UART等，它具有性能可靠、性價比高等特點。在工業生產中，往往需要機械手臂完成一些相對復雜的控制運動，機械手臂系統中一個單片機遠遠不夠，仍需要TK-A66自由度機械手臂、1501數字舵機等控制機械臂的運動。

本文就是用單片機作為核心控制器，LD1501-MG數字舵機作為TK-A66自由度機械手臂的運動控制單元，結合制成的6自由度機械臂的控制系統。系統采用單片機芯片STC89C52作為主控制器，A/D轉換采用以雙積分方式運行的ICL7135，利用定時計數器的計數功能，測量外部電壓，省去很多處部電路[2]。單片機通過產生PWM信號控制機械臂的舵機，從而控制6自由度的機械手臂。

二、算法設計

（一）建立運動學模型

如圖1所示，具有三維開環鏈結構的機械手分別由基座、增幅臂和旋轉手臂組成，可通過肩部旋轉和俯仰、肘部和腕部俯仰、腕部偏轉和旋轉6個旋轉關節實現6個自由度的運動，進而保證機械手在工作空間中的所有位置狀態[3]。

圖1機械臂模型

為準確反映關節坐標與機械端位置之間的關系，本文根據D-H原則和笛卡爾坐標系建立機械臂關節的坐標系及機械臂的正向運動學方程，但逆運動學模式在實際應用過程中更為普遍[4]。機械臂控制系統的運行可以通過調整關機位置來實現，這就需要通過逆運動學模型，計算出完成理想軌跡時每個關節所需達到的角度值。在確定了機械臂的構型后應以D-H法則確定運動學參數，如表1所示。

表1 D-H參數表

根據圖1中的構型建立參考坐標系和關節坐標系，再用齊次方程描述機械臂各連桿的空間幾何關系，用4×4齊次矩陣來表示相鄰連桿之間的幾何關系，再算出機械臂相對于參考坐標系的后端地址。鑒于該坐標系中存在偏移量α1和α2，故χ1、χ2和χ3的空間關系并不重合，如圖2所示。

圖2 1,2,3坐標空間關系

（二）運動控制算法

運動軌跡和速度是機械臂兩大控制變量，軌跡控制算法是機械臂控制系統的最關鍵部分。相對于二維軌跡控制算法，六自由度軌跡控制算不是二維插補算法的簡單組合，綜合曲線特征，采用分段曲線逼近擬合軌跡曲線。為了提高擬合度，須精確計算出各段曲線交點值，以保證運動狀態參數的連續性[5]?？紤]到既要使機械臂能沿著既定的軌跡運動，又要確保運動過程中的實時可控性，本文采用Trial Mode運動模式的控制算法，即通過階梯形分割其速度曲線，在每個采樣周期內分段進行提取設計，確保對插補算法指定軌跡進行約束。

（三）調試運動規劃算法

合理設計機械臂算法調試軟件，本文軟件采用C語言編寫算法，借助WinCE系統平臺構建其運動規劃算法的實驗平臺，采用MFC設計合理的程序框架，來驗證算法功能以確保算法函數的可行性與控制的精準度，確保對不同運動模式的仿真驗證。

三、硬件設計

將硬件設計過程分為單片機模塊和舵機控制模塊兩個部分，系統原理圖如圖3所示。在機械臂硬件設計過程時，其構成部件的選擇應匹配于機械臂的基本技術參數，之后根據對關機轉矩等計算推演，合理選擇滿足功率要求的電機。綜合考慮，本硬件設計中核心的器件選用STC89C52型號的微處理器，內置8KB可在線編程閃存，具有A/D轉換功能，并能產生PWM信號。另一方面，產生的PWM信號控制機械臂舵機的制動[6]。

圖3系統原理圖

（一）單片機模塊

單片機模塊所使用的STC89C52單片機，包括單片機芯片、復位電路、時鐘電路和電源電路。該芯片采用CMOS工藝，具有密度高、存儲信息不易丟失的特點，與C51指令集兼容。Internal RAM允許重復在線編程，還可通過SPI串行端口或同一非易失性存儲器進行重新編輯。通過將8位CPU與可線上下載的Flash進行集成，可有效地解決復雜的控制問題。內部系統包括8K字節可在線編程ISP閃存，4.0-5.5V電源，完全靜態工作0Hz～24MHz，256字節內部RAM，32根可編程I/O線，8個中斷源和雙數據指針，2個16位定時器/計數器。

（二）舵機控制模塊

本系統采用數字舵機LD1501-MG分2組進行實驗，第一組中舵機與單片機共用同一電源，第二組中使用不同電源供電。數據表明，第一組單片機運行不穩定，第二組中兩者運行較好。所以本系統采用舵機與單片機公用不同電源方案，且舵機所需的電源功率要大于單片機的電源功率，如圖4所示[7]。

2）其他土地利用類型都有不同程度的減少，其中水域和耕地減少的最為明顯，1985－2000年和2000－2016年水域的減少速度分別為0.08%和0.19%，耕地的減少速度分別為0.12%和0.14%；

圖4舵機驅動電路圖

四、軟件設計

（一）總體設計

本設計利用上位機操作系統及監控程序實現信息通訊、軌跡規劃及運動算法控制等功能[8]。根據操作步驟，上位機提取INI文件和數據庫中的參數，并在可視化界面中輕易完成機械臂軌跡規劃和關節角位置的設定。信息通信過程首先要初始化通信端口，通信總線與各關節進行通信連接，發出連接信號并讀取數據，進而發出指令并驅動電動機運動。為兼顧系統軟件功能可以同步完成，本控制系統采由C51語言編程來實現，程序結構采用模塊式，由主程序、初末置換函數、速度控制函數、機械臂控制函數、定時器T0子程序、定時器T1子程序6部分組成。結構清晰，便于分析調試。

（二）主程序設計

主程序是將整個程序分成若干模塊，在程序中利用函數調用的方式，連接各個子程序，傳遞之間的參數。系統的主程序按照以下流程執行。

程序開始，對全局變量進行聲明，對相關數據及參數初始化，3個定時器數據初始化，將輸出引腳設置為強推挽輸出，為舵機提供足夠的電流。中斷設置，定時器初始化并開啟中斷，設置定時器TCON和TMOD參數。

中斷請求，接到信號請求則調用pwm[]數組更新函數舵機調速子程序vpwm()。流程圖如圖5所示。

（三）舵機控制程序設計

圖5程序流程圖

為保證機械臂平穩運行，需要對舵機進行精確控制，以實現速度調節，實際上在程序中對舵機控制信號PWM占空比進行調節即可。PWM是脈沖寬度調制信號，具有易控制、迅速和動態響應好等特點，被廣泛應用在人工智能機器人及航模設計的舵機及直流電機的控制中，其PWM設置格式應注意以下幾點：

（1）上升沿保證有0.5ms，介于0.5ms至2.5ms之間；

(2)要求用程序實現連續的PWM信號，對于硬件體現出來的機械臂動作更加連續、流暢。

圖6多路舵機PWM速度控制程序流程圖

五、系統仿真與實現

為使控制系統的開發更加的高效便捷，在進行硬件電路的焊接前，先進行仿真。本控制系統設計，用Protues仿真軟件搭建仿真系統的電路圖，由于舵機在Protues元器件庫中沒有加入，本仿真可用Protues自帶的虛擬示波器Oscilloscope代替。通過觀察Oscilloscope顯示的PWM控制信號的占空比，判斷控制系統的控制性能?？筛鶕抡娼Y果，通過調整單片機程序，改變占空比，從而得到電機平均速度，達到調速的目的。

硬件調試階段首先測試舵機，將舵機測試程序燒錄到單片機系統中，測試并記錄舵機偏轉的角度并與預期角度進行比較，誤差角度控制在±1度以內，對不能達到要求的舵機不予選用。選擇好舵機以后根據機械臂結構圖組裝好機械臂，通過軟件編程程序控制機械臂實現6自由度運動。最后，組裝硬件并反復調試。在調試的過程中，根據實際情況，機械臂的重量，平穩程度等，可通過編程來控制發送PWM信號的時間，以適應不同距離的測量需要，以達到準確抓取目標的任務，確保實現6自由度機械臂的運動控制功能。

六、結語

機械臂作為應用廣泛的運動執行機構，其運動穩定性在現代工業生產中意義重大。本方案在反復比較國內外同類產品的基礎上，深入分析其適用行業特點和功能需求，確定總體設計方案使用STC89C52作為主控制器、LD1501-MG數字舵機作為控制單元，主要由機械臂電路與單片機控制電路兩大部分構成。

機械臂控制系統中建立運動學模型，運用TM算法，對機械臂手臂的運動速度和軌跡進行控制設計，實現機械臂手臂運動軌跡的控制功能。在本系統控制程序中，利用單片機產生的PWM信號對舵機進行控制，給出了微分插補法生成多路舵機PWM速度控制信號的程序實現方法，通過微分插補法進行舵機速度的控制，實現機械臂狀態的控制和調整，使機械臂的運行更加平穩。此系統的仿真實現受限于無障礙的空間環境，其控制和軌跡算法應結合不同的復雜環境進行跟進調整，使其功能和應用價值更為凸顯。