李晟鍇

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

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

1 機械臂關(guān)鍵技術(shù)的發(fā)展

1.1 六自由度機械臂控制系統(tǒng)的研究現(xiàn)狀

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

1.2 六自由度機械臂軌跡規(guī)劃的研究現(xiàn)狀

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

2 六自由度機械臂控制系統(tǒng)的算法設(shè)計

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

如圖1所示，機械臂的機械結(jié)構(gòu)具有六個自由度，屬于三維開環(huán)鏈式結(jié)構(gòu)，從上到下分別由基座、富養(yǎng)臂以及旋轉(zhuǎn)臂構(gòu)成，這些部分分別由六個旋轉(zhuǎn)關(guān)節(jié)連接，即肩部回轉(zhuǎn)、肩部仰俯、肘部仰俯、腕部仰俯、腕部偏轉(zhuǎn)以及腕部回轉(zhuǎn)。通過對這六部分的控制就能實現(xiàn)機械臂在工作空間內(nèi)的全部位置狀態(tài)。

圖1 機械臂模型

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

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

圖2 各關(guān)節(jié)坐標系

圖3 1、2、3坐標系空間關(guān)系

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

表1 D-H參數(shù)表

2.2 六自由度機械臂運動規(guī)劃算法的設(shè)計

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

2.3 六自由度機械臂運動規(guī)劃算法的調(diào)試設(shè)計

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

3 六自由度機械臂硬件系統(tǒng)的設(shè)計

在機械臂的設(shè)計過程中，考慮到系統(tǒng)的機電耦合關(guān)系，將設(shè)計過程分為機械結(jié)構(gòu)設(shè)計和控制系統(tǒng)設(shè)計兩個部分，在硬件設(shè)計過程時，需要先擬合機械臂的基本技術(shù)參數(shù)篩選組成部件，然后再根據(jù)對關(guān)機力矩的推算，選擇滿足功率要求的電機。

(1)機械臂構(gòu)型的選擇

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

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

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

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

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

3.1 電源電路

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

3.2 時鐘電路

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

3.3 外接SRAM電路

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

3.4 CAN總線

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

4 六自由度機械臂軟件系統(tǒng)的設(shè)計

(1)PID控制算法

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

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

(2)通信軟件設(shè)計

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

(3)上位機軟件設(shè)計

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

5 結(jié)語

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

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

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

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

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