六自由度機械臂控制系統設計與研究

李晟鍇

(淮南聯合大學 機電系，安徽 淮南 232001)

對六自由度機械臂控制系統的研究，主要是為了使機械臂可以按照人們既定的要求完成規定動作，這就需要對機械臂的動作軌跡進行規劃，接著在各個關節軸采取位置控制的方式來驅動機械臂完成預期的運動目標，其中控制算法的選擇是保證機械臂動作平順性和準確性的關鍵步驟。控制算法的選擇設計一般需要基于機械臂系統的運動學模型，此外，既考慮到系統控制的簡單性，又考慮到系統的運動狀態，本文中建立了六自由度的機械臂動力學模型，并在此基礎進行機械臂控制系統的設計與研究。

1 機械臂關鍵技術的發展

1.1 六自由度機械臂控制系統的研究現狀

六自由度機械臂通常由多個電機協同聯動控制，為了使不同電機之間實現有效通信，需要在主控制器與各運動控制單元之間建立數據通訊系統，目前，常用的控制體系結構包括中央式和分布式兩種。中央式體系結構是較為傳統的控制方法，可以滿足大部分系統的控制要求，但當中央控制器出現故障時，會對整個控制體系產生嚴重影響。同時，由于中央式體系結構復雜，給后期的維護造成困難。分布式控制體系是將控制信號分配到各控制節點的微處理器，在這種控制體系下，主控制器只需要實現總線連接和機械臂運動學算法的功能，可以簡化控制設備投入，提高了整個系統的控制穩定性。

1.2 六自由度機械臂軌跡規劃的研究現狀

六自由度機械臂的軌跡規劃是保證機械臂準確動作和穩定控制的基礎，有利于提高機械臂的運動效率，并能改進系統的跟蹤誤差。通常情況下，用性能優化指標來描述機械臂軌跡規劃性能。以對時間最優下的機械臂軌跡規劃算法的研究較為成熟，根據軌跡規劃的重點不同，可以分為沿預設路徑下的最優時間動作軌跡算法和最優時間下的點到點化處理算法。沿預設路徑下的最優時間動作軌跡算法考慮機械臂的位置、速度、加速度及二階加速度的因素，滿足要求的軌跡曲線可以用連接機械臂關節空間內關鍵點的高階多項式曲線來獲得。最優時間下的點到點化處理算法主要是在空間內抽象多個工作點，并要求機械臂關節在各工作點運動來完成預定任務，各關節的運動線性獨立且需在工作點具有停留時間，目前常用的方法是在各節點間采用多項式插值的方法來進行軌跡規劃。

2 六自由度機械臂控制系統的算法設計

2.1 機械臂六自由度運動學模型的建立

如圖1所示，機械臂的機械結構具有六個自由度，屬于三維開環鏈式結構，從上到下分別由基座、富養臂以及旋轉臂構成，這些部分分別由六個旋轉關節連接，即肩部回轉、肩部仰俯、肘部仰俯、腕部仰俯、腕部偏轉以及腕部回轉。通過對這六部分的控制就能實現機械臂在工作空間內的全部位置狀態。

圖1 機械臂模型

本文中對六個旋轉關節根據右手法則及D-H原則建立坐標系，然后根據機械臂兩兩相鄰的桿件之間的空間幾何關系，可以建立六自由度機械臂正運動學方程，從而可以準確地反映出關節坐標與機械末端位置的關系，但在實際應用過程中，逆運動學模式更為普遍。機械臂控制器可以根據逆運動學模型求出在完成理想運動軌跡時各個關節所需要達到的角度值，并通過對關機位置的整定實現對機械臂控制系統的控制。

在機械臂構型選擇后，需要對運動學參數進行確定，根據圖1機械臂構型建立機械臂的參考坐標系及關節坐標系如圖2所示，由于坐標系存在偏移量a1和a2，所以1、2和3坐標系的x軸并不重合，x1、x2和x3的空間關系如圖3所示。

圖2 各關節坐標系

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

六自由度機械臂的運動學參數應包括繞z軸的旋轉角θ、z軸上兩條相鄰公垂線的距離d、公垂線的長度a及兩個相鄰z軸間的夾角α。本文中以D-H法則來對運動參數進行確定，以齊次方程來描述機械臂各連桿相對于參考坐標系的空間幾何關系，用4×4齊次矩陣來描述相鄰連桿之間的幾何關系，進而就可以推導出相對于參考坐標系的機械臂末端位置，本設計方案中機械臂的D-H參數選擇如表1所示。

表1 D-H參數表

2.2 六自由度機械臂運動規劃算法的設計

軌跡規劃是機械臂運動控制中的重要工作，在不同運動方式下，需要采用不同的軌跡規劃算法，規定合適的軌跡曲線是一種最為簡單的軌跡規范算法，但這種單一型的軌跡規劃算法不能滿足機械臂運動過程中的剛性和柔性沖擊，在隨后的發展過程中，根據各種規劃曲線的特點設計出一種合成型軌跡規劃曲線，這種軌跡規劃算法通常采用分段函數的形式，在函數每段曲線的拐點值需要滿足運動狀態參數的連續。本文中采用了Trial Mode運動模式的控制算法，其集中了傳統的Auto Mode和Teach Mode運動模式，不僅使系統可以沿著既定的運行軌跡運動，還可以保證在此過程中系統實時規劃可控。Trial Mode運動模式在設計過程中選擇分割的梯形速度曲線，并要求在每個采樣周期內分段進行規劃。此外，還應對插補算法規定軌跡約束。

2.3 六自由度機械臂運動規劃算法的調試設計

在六自由度機械臂控制算法設計完成之后，在搭建機械臂實驗平臺之前，需要編寫機械臂算法調試軟件，對算法函數進行解算驗證，來保證算法函數的可行性與控制邏輯的準確性。本文中在WinCE系統平臺搭建機械臂控制系統，并采用C++語言編寫算法代碼方便在控制器內運行，在軟件框架的選擇上采用MFC框架，設計合理的程序框架，其功能需要包括機械參數、工具參數、運行參數以及坐標系參數，并能保證不同運動模式的仿真驗證。

3 六自由度機械臂硬件系統的設計

在機械臂的設計過程中，考慮到系統的機電耦合關系，將設計過程分為機械結構設計和控制系統設計兩個部分，在硬件設計過程時，需要先擬合機械臂的基本技術參數篩選組成部件，然后再根據對關機力矩的推算，選擇滿足功率要求的電機。

(1)機械臂構型的選擇

為了保證機械臂可以在工作空間內完成期望的動作要求，可以將物品抓持到準確位置，并能進行姿態移動，就需要使機械臂滿足自由度的條件。機械臂構型的選擇是機械臂設計的基礎，自由度越高，機械臂靈活性越高，但同時控制結構也更為復雜。因此，應使機械臂自由度與任務相匹配，本文中在機械臂六自由度運動學模型基礎上設計具有六個旋轉關節的機械臂，為了使機械臂存在封閉解，需要使相鄰的三個關節軸線相交于一點，因此，定位結構選擇轉角為0°或90°的簡單結構連桿，機械臂六個關節分別由六個力矩電機來驅動。

(2)機械臂驅動電機的選擇

可供機械臂關節使用的驅動電機種類繁多，本設計選擇構型簡單且控制方便的直流有刷伺服電機。此外，為了滿足機械臂不同類型的運動狀態，還需要考慮到電機的最大承載力矩和各個關節的最大角速度，通常情況下，電機峰值堵轉功率是電機選擇過程中的重要參考指標。各關節的動力參數要求是選擇各關節驅動元件及傳動元件的依據，可以采用靜力學方法和動力學方法計算機械臂的動力參數，其中靜力學方法是忽略負載的動態影響而只計算靜負載；動力學方法是在計算過程中同時設計到靜負載和動負載。假設關節理論節點中心承載關節重點，連桿中心承載連桿重量，之后即可計算各關節所需力矩。

(3)機械臂控制器的選擇

本設計方案中采用DSP TMS320LF2407作為主控制芯片，其具有40MHz時鐘頻率且片內資源豐富，其內置的事件管理器模塊可以滿足驅動電機的控制要求，此外，芯片的尺寸小也可節省電路板的空間。為了提高系統的動態控制性能，采用包含位置環與速度環的雙閉環控制方式，控制者可以在工控機輸入機械臂的預期控制方案，控制器通過算法計算理想的軌跡規劃曲線，并換算為增量碼盤的脈沖數。實際脈沖數與理想脈沖數的偏差作為關節控制器的輸入，積分分離PID控制器求解系統的位置控制量，可以反映為PWM波形不同的占空比，進而可以保證電機驅動器控制電機運行。

3.1 電源電路

控制芯片需要在穩定的3.3V供電電壓下工作，通過開關電源后接入B050LS模塊，進而可以保證產生的3.3V電壓具有穩定的輸出，防止電壓不穩而對控制芯片工作狀態的影響，DSP TMS320LF2407的5管腳和6管腳接入供電電壓，8管腳接上電復位信號。

3.2 時鐘電路

雖然DSP TMS320LF2407內部含有振蕩器，但為了提高時鐘精度和控制穩定性，一般在外部接入有源晶振，由于機械臂控制系統對時鐘質量要求較高，選擇4管腳的有源晶振。另外，DSP TMS320LF2407可運行鎖相環模式，對外部時鐘源進行倍頻，得到穩定的時鐘信號。

3.3 外接SRAM電路

DSP TMS320LF2407芯片最多可尋址64K的外部程序空間和64K的外部數據控制，本設計方案中控制算法需要一定的存儲空間，DSP TMS320LF2407內含的存儲空間不能滿足要求，因此，系統采用外部擴充RAM CY7C1021V33，其存儲大小為64K*16bit，該芯片具有可在全周期內快速運行的優點。

3.4 CAN總線

DSP TMS320LF2407 中通過PCA82C250來驅動通信接口，滿足系統差動收發功能，本設計中設計電平轉換模塊來保證PCA82C250的5V電源供電電壓，為了對電路進行簡化，采用最簡單的電阻分壓來實現電平轉化，為了提高數據通信過程中抗干擾性和可靠性，還需要在該模塊中串接具有快速恢復能力的肖特基二級管1N5219和兩個終端匹配電阻。

4 六自由度機械臂軟件系統的設計

(1)PID控制算法

DSP TMS320LF2407為主控芯片，速度環與位置環雙閉環都采用積分分離PID控制，此外，為了保證系統的控制穩定性，還需要根據實際運行環境調試系統的控制律。在確定電機絕對位置時，有效結合零位霍爾接近開關和增量編碼器Z通道的信號輸出。PID控制方法具有原理簡單、使用方便、適用性強和魯棒性強的優點，PID控制器的輸入e(t)輸出u(t)之間的關系為：

其中，比例調節參數Kp的作用是對系統調整以減小超調量，同時，對于調節速度也有控制作用，但當參數過大時會使系統穩定性降低。積分調節參數KI的作用反應在對穩態誤差的消除上，只要誤差存在積分調節就會持續進行，直到系統達到誤差狀態。微分調節參數KD可以反應于系統偏差的變化，實現對系統的超前控制，對于改善系統的動態性能大有裨益。

(2)通信軟件設計

機械臂在控制過程中不僅需要與六個下位機進行通信，還需要與I/O板進行信息交互，從而實現對推進裝置及液壓裝置的控制，這些部分之間都需要進行CAN通信，CAN通訊網絡中采用數據幀類型、標準幀格式，其中，上位機下傳的數據包括各關節期望角度、期望速度、各個關節所受的重力矩、PID參數、I/O控制量等，而接受的數據主要包括當前機械臂各軸合成角度、推進裝置當前位置及位移傳感器數值等。下位機之間不進行通訊交流，而下位機都與上位機交互，因此，可以用上位機采用點對點主從的協調通信方式協調管理7個下位機，上位機為主動模式，下位機為從動模式，只有當下位機接受到上位機發送的幀數據或指令時，才會相應返回數據。

(3)上位機軟件設計

本設計方案中以PC機為上位機，在Windows XP操作系統及C++平臺設計主計算機監控程序，從而實現信息通訊、軌跡規劃及運動算法控制等功能。上位機可以響應界面操作，從INI文件及數據庫中提取參數，并能根據用戶要求對機械臂進行軌跡規劃和關節角位置設定，同規格通信總線與各關節進行通信，發出控制指令，驅動電機運動。為了保證系統軟件功能可以同步完成，采取多線程的方法進行設計，響應桌面功能作為主線程，當軟件啟動后就開始工作，直到軟件關閉，其間把消息分發給相關組件或對象。路徑規劃功能作為一個子線程，得出的規劃結果存入全局變量數組中，提供給其他部分調用，通過控制若干全局變量標志位來實現對該線程的轉向。通信功能可以采用系統提供的定時器空間來實現，與各個關節控制器交換數據時，可以進行簡單的保留，以方便后期的網絡導出。

5 結語

機械臂是一種在現代工業中廣泛應用的執行機構，對于保證工業生產的穩定性有重要意義。本設計方案中根據機械臂在工作空間中的動作特點，設計了六自由度鏈式關節結構機械臂，并通過估算各個關節的力矩實現對機械臂關機所需電機的選型，為了實現各個部分之間的有效通信，將工控機與關節控制器掛在CAN總線上。在系統建模方面，采用D-H建模方法，通過逐次解耦的方向推導出了逆運動學模型，采用積分分離PID控制方法對系統的狀態性能進行調節，實現對系統反應速度、超調量以及穩態性能的提升。

[1] 宋婧.六自由度機器人的運動優化和軌跡規劃算法研究[D].武漢：武漢科技大學，2011.

[2] 譚民，徐德，侯增廣.先進機器人控制[M].北京:高等教育出版社，2007.

[3] 錢東海，王新峰，趙偉，等.基于旋量理論和Paden-Kahan子問題的6自由度機器人逆解算法[J].機械工程學報，2009(9): 72-76.

[4] 彭圣明，裴海龍，王清陽.基于機械臂的運動學研究與應用[J].機械設計與制造，2011(7):136-138.