基于模糊PID的懸吊式機械臂重力補償控制系統(tǒng)設計

李崇智

(陜西鐵路工程職業(yè)技術學院，陜西 渭南 714099)

0 引言

機械臂具有點膠速度快、高精度的特點，可實現(xiàn)重復作業(yè)，因此在諸多領域具有較高的應用價值[1]。懸吊式機械臂是常用機械臂的一種，處于移動狀態(tài)的機械臂受到重力作用會產生垂直向下的力，當機械臂執(zhí)行上下移動動作時，需要消耗更多電能，而當機械臂的移動重力高于牽引力時，機械臂可能出現(xiàn)移動失衡的情況，不僅會影響其移動效率和工作質量，嚴重時還會導致機械臂掉落，引發(fā)重大的安全事故。

為保證懸吊式機械臂的工作安全性，同時提高機械臂的工作效率，設計并開發(fā)了懸吊式機械臂重力補償控制系統(tǒng)。現(xiàn)階段發(fā)展較為成熟的研究成果包括：基于軌跡規(guī)劃的補償控制系統(tǒng)、基于最優(yōu)激勵位姿序列的補償控制系統(tǒng)以及基于逆動力學的補償控制系統(tǒng)。然而現(xiàn)有的補償控制系統(tǒng)在實際應用過程中存在控制效果不佳的問題，主要體現(xiàn)在機械臂重力補償不足或過量兩個方面，最終導致機械臂的穩(wěn)定性下降?；诖?，國內外學者均對此進行研究，如Perrusquia A等人[2]提出基于重力補償控制器的機器人局部放電整定新方法。該方法將局部放電控制與重力補償相結合，提出了一種新的方法來調整PD+G控制器。利用La-Salle定理的全局漸近穩(wěn)定性結果和機器人動力學特性獲得了調諧方法。該方法對于機器人的局部放電控制效果良好，但是其對于重力補償?shù)难芯坎蛔?。又如Lin H等人[3]提出了一個通用和系統(tǒng)的框架，以解決dVRK機器人手臂的MTM重力補償問題。該方法為避免殘余誤差在質量分布不均勻的MTM鏈路之間傳遞，依次對每個鏈路的重力和干擾力參數(shù)進行識別，最終完成重力補償控制器的設計，來補償機械臂的重力和干擾力。該方法具有一定有效性，但是其對機械臂的重力及干擾力參數(shù)分析仍然不足，其重力補償效果未有較大改進。為此，本次研究在現(xiàn)有控制系統(tǒng)的基礎上，引入模糊PID算法。

模糊PID算法融合了模糊控制理論和PID算法，其中模糊理論是利用模糊集的基礎概念或者是連續(xù)從屬函數(shù)來實現(xiàn)的，將其與PID算法的融合，可規(guī)避兩種算法在單獨使用時的弊端，實現(xiàn)目標的最優(yōu)控制。將模糊PID算法應用到懸吊式機械臂重力補償控制系統(tǒng)的優(yōu)化設計工作中，以期能夠提高機械臂的重力補償控制效果。

1 懸吊式機械臂重力補償控制硬件系統(tǒng)設計

要想實現(xiàn)懸吊式機械臂重力補償，先根據懸吊式機械臂當前工作狀態(tài)，確定重力補償量，進而確定補償裝置移動的物理量，保證懸吊式機械臂處于平衡狀態(tài)。

1.1 主控制器

系統(tǒng)應用了模糊PID算法，該控制器所處理的數(shù)據較為復雜，所以主控制器選用了型號為STM32F103VET6的控制設備，該設備內置了512 kB Flash芯片，可實現(xiàn)模糊PID控制，同時還能存儲所需數(shù)據[2]。主控制器結構如圖1所示。

圖1 系統(tǒng)主控制器結構圖

主控制器內置的多個16位計時器協(xié)同中斷，生成一定占空因數(shù)的 PWM波，其通用 I/O端口和 IIC接口可與外部設備相連，滿足機械臂重力補償控制程序的運行要求。主控制器中的CAN端口由TJA1050CAN控制器接口模塊TJA1050完成，每臺控制器的通訊速度可達1 Mbps，利用Ethernet總線與PC機進行通訊時其最高速率可達到100 Mbps。

1.2 傳感器

系統(tǒng)采用模糊 PID 控制，屬于一種閉環(huán)控制器，它要求對運動角進行測量和反饋，以便于計算運動所需要的角度偏差。系統(tǒng)中安裝的傳感器設備分為拉力傳感器和機械臂工作角度傳感器兩個部分，拉力傳感器的檢測精度不得低于懸吊式機械臂最大拉力的千分之一，保證吊絲拉力變化的檢測質量，根據拉力變化值來實現(xiàn)重力補償控制，為滿足上述要求選擇RSS01型號的拉力傳感器設備[4]。選擇MPU6050型傳感器實現(xiàn)機械臂各個關節(jié)的動態(tài)角實時檢測。MPU6050是一種集成式六軸移動處理裝置，它包括陀螺儀、加速度計以及數(shù)字處理器，可通過I2C接口與傳感器相連接。角度傳感器的測量范圍是可控的，陀螺儀的測量范圍能夠達到每秒±2 000°，可準確追蹤機械手臂關節(jié)的運動角度。

1.3 重力補償裝置

根據機械手的工作要求，采用力矩機構來實現(xiàn)對機械臂的重力進行補償，當懸吊式機械臂向上運動時，重力補償裝置會產生與其力度大小相同、方向相反的力矩，通過該裝置帶動鋼絲繩，使之達到平衡狀態(tài)[5]。所設計的重力補償裝置結構如圖2所示。

圖2 重力補償裝置組成結構圖

圖2中，恒拉力裝置由力矩電機、卷筒和吊絲等部分組成。吊絲定位裝置是將懸絲橫向固定在水平位置，以減少因卷軸旋轉而引起的橫向偏移[6]。保護機構是用一段鋼絲繩組成，繩索一端與地面相連，另一端與試驗件下端相連。其作用是防止試驗品在高速垂直移動時不小心碰撞到電機，避免重力補償器的損傷。

1.4 驅動電機

驅動電機的選型受到機械臂的體積和重量的限制，通常要求結構尺寸較小，系統(tǒng)采用MaxonEC-4pole直流無刷電機，可為系統(tǒng)提供60 V的輸入電壓、20 A的連續(xù)電流輸出[7]。所安裝的驅動電機支持遞進型編碼器，通過編碼器接口將懸吊式機械臂各個關節(jié)的動作信息反饋給主控制器，并能支持閉環(huán)轉速控制和位置控制，利用驅動器內部的電流回饋，實現(xiàn)閉環(huán)電流控制[8]。同時，該驅動電機支持串口通信和CANOpen總線通信，接口電路可為驅動器通信及供電提供便利。

1.5 通信模塊設計

根據圖3結構可將系統(tǒng)通信鏈路分為三部分，分別為上位機和主控制器通信鏈路、主控制器和節(jié)點控制器之間的通信鏈路及節(jié)點控制器和驅動器、傳感器之間的通信鏈路。

圖3 控制系統(tǒng)通信鏈路框圖

采用以太網接口連接的方式實現(xiàn)上位機與主控器之間的通信[9]。節(jié)點控制器指的是機器人的各個關節(jié)控制節(jié)點，它們之間的通信鏈路由 CAN總線進行，通訊波特率為500 kHz，而節(jié)點控制器和傳感器終端之間的通信采用的是無線通信方式。

2 懸吊式機械臂重力補償控制系統(tǒng)軟件功能設計

2.1 構建懸吊式機械臂數(shù)學模型

如圖4所示為懸吊式機械臂的結構模型，其主要由4個轉動關節(jié)、兩段長臂桿和一個末端執(zhí)行器組成。

圖4 懸吊式機械臂結構模型

如圖4所示，模型中手臂關節(jié)決定手臂末端的空間位置，而肘關節(jié)、腕關節(jié)和指間關節(jié)決定手臂末端的空間姿態(tài)[10]。根據機械臂上各關節(jié)之間的空間關系，得出機械臂的坐標轉換矩陣為：

(1)

式中,n、s、a和p分別對應的是法向向量、滑動向量、接近向量、末端位置坐標，角標x、y和z對應的是機械臂在空間環(huán)境中的三維分量[11]。通過公式(1)的變換，得出懸吊式機械臂執(zhí)行不同動作時的運動學方程，將運動學方程與機械臂結構模型相結合，得出懸吊式機械臂數(shù)學模型的構建結果。

2.2 檢測懸吊式機械臂實時位姿

在構建的懸吊式機械臂數(shù)學模型下，根據機械臂不同姿態(tài)的運動學特征，將機械臂的位姿描述為：

{B}={RBPB}

(2)

式中,RB和PB分別表示機械臂的旋轉矩陣和位置矩陣[12]。利用硬件系統(tǒng)中的傳感器設備，獲取當前機械臂任意關節(jié)的橫向繞度為：

(3)

其中:qik(t)和φik(x)分別表示機械臂關節(jié)i的k階模態(tài)坐標和模態(tài)坐標的振型函數(shù)，φik(x)的函數(shù)表達式如下：

(4)

式中,θ表示傳感器采集到的機械臂關節(jié)的旋轉角度，Li表示當前關節(jié)與其相鄰關節(jié)之間的長度值。按照上述方式可得出機械臂任意關節(jié)的縱向繞度求解結果，即可確定機械臂所有關節(jié)的位置坐標[13]。將求解出的機械臂關節(jié)位置坐標與傳感器采集的角度信息代入到公式(2)中，即可得出當前機械臂的位姿檢測結果。

2.3 建立懸吊式機械臂重力平衡方程

根據能量守恒定理，懸吊式機械臂在不同工作狀態(tài)下其勢能與動能之和始終處于恒定狀態(tài)，在已知懸吊式機械臂位姿的前提下，可得出機械臂的動力勢能為：

(5)

公式(5)中變量Tr為動能，Uip和Uir均為常數(shù)，其計算公式如下：

(6)

其中:Ai表示關聯(lián)第i個坐標系和基坐標系間的齊次坐標變換式，aj以及公式(5)中的ap和ar分別表示不同機械臂關節(jié)的加速度，Ii表示慣性張量[14]。機械臂勢能可表示為：

(7)

(8)

式中,νi和τi分別表示懸吊式機械臂關節(jié)i的速度矢量和驅動力矩。在實際重力補償與控制過程中，忽略機械臂工作環(huán)境中的磁場勢能、電勢能和分子勢能，只考慮重力勢能[15]。若懸吊式機械臂執(zhí)行的移動任務涉及關節(jié)旋轉，則需要在動能計算過程中考慮關節(jié)角位置和角速度。

2.4 計算機械臂負載力矩

假設懸吊式機械臂在當前位姿狀態(tài)下所承受的牽引力為F，牽引力的作用點為Q(xF,yF,zF)，則牽引力作用在機械臂上的負載距離表達式：

(9)

式中,xF、yF和zF對應的是牽引力在3個方向上的分量，設力作用點Q的矢徑為R，則牽引力對機械臂重心點的力矩可表示為：

Mo(F)=R×F

(10)

公式(10)中力矩矢量Mo(F)的大小和方向都與機械臂重心的位置有關[16]。機械臂上各個關節(jié)的力矩可被看作是額外的彎矩和反向力矩之和[17]。

2.5 利用模糊PID算法求解機械臂重力補償控制量

模糊PID控制算法的控制性能取決于它的控制規(guī)則，控制規(guī)則的數(shù)量與控制器輸入數(shù)量之間存在指數(shù)關系。在系統(tǒng)運行過程中，輸入量為誤差和誤差變化率。模糊PID算法控制原理如圖5所示。

圖5 模糊PID算法控制原理圖

在機械臂重力補償控制過程中，模糊語言變量中必須選取適當?shù)碾`屬度函數(shù)[18]。要注意功能曲線的形狀和分布對控制性能產生的影響。將模糊處理后的變量代入到公式(11)中，計算控制器的輸入值。

(11)

式中,i(t)和o(t)分別表示設定值和實際輸出值，計算結果e(t)和ec(t)分別表示誤差和誤差率，t表示時間。將偏差的比例系數(shù)κp、積分系數(shù)κi和微分系數(shù)κd進行調整，組合構成控制量。κp反映了控制器的輸出和輸入偏差之間的比例關系，增加比例系數(shù)可降低系統(tǒng)的穩(wěn)定誤差，從而改善系統(tǒng)的穩(wěn)定狀態(tài)[19]。積分系數(shù)主要是為了消除穩(wěn)態(tài)誤差，隨著時間的推移，穩(wěn)態(tài)誤差會逐漸降低，最后達到0。微分系數(shù)的功能是提高控制器的運行穩(wěn)定性。通過對模糊PID控制器工作參數(shù)的設置與調整，可以得出控制器的離散控制規(guī)律為：

(12)

將公式(11)的計算結果代入到公式(12)中，即可得出PID控制器的輸出結果u(t)。受到懸吊式機械臂工作狀態(tài)以及位姿的影響，輸入控制器中的數(shù)據呈現(xiàn)出動態(tài)變化規(guī)律，要求控制器的工作參數(shù)隨之發(fā)生變化，因此需要創(chuàng)建模糊規(guī)則，并調整PID控制器的運行參數(shù)，PID控制器運行參數(shù)的調整規(guī)則如下：若輸入的誤差值e較大，向上提高比例系數(shù)κp的值，同時降低積分系數(shù)κi和微分系數(shù)κd的具體取值，改善了系統(tǒng)的響應速度并避免超調，并有效地抑制了由誤差改變率引起的微分過飽和；如果輸入的e較大，不調整κp和κi的值，κd參數(shù)小幅度降低，同時兼顧了控制的精確度與穩(wěn)定性，并確保了響應速度[20]。當輸入的誤差值過小時，降低κp值的同時增大κi參數(shù)，在減少振蕩的同時消除系統(tǒng)的靜態(tài)誤差，在考慮誤差變化率的情況下確定參數(shù)κd的調整方向。將調整后的參數(shù)代入到公式(12)中，得出模糊PID控制器的輸出結果。最終把模糊PID控制器輸出的數(shù)值轉換成可以控制執(zhí)行元件的精確量，去模糊化處理過程可表示為：

(13)

其中:ui表示模糊PID控制器的輸出結果，δi表示模糊變量值，在懸吊式機械臂重力補償量的求解過程中，將機械臂的負載力矩計算結果作為輸入量導入到控制器中，在考慮機械臂工作穩(wěn)定性的情況下，得出重力補償控制量為：

(14)

公式(14)的求解結果為機械臂重心點的重力補償控制量，按照上述方式可求解出機械臂上各個關節(jié)的重力補償控制量。

2.6 實現(xiàn)懸吊式機械臂重力補償控制

在懸吊式機械臂設備上設置多個重力補償點，將重力補償設備安裝在各個測點位置上，根據機械臂實時力矩以及補償控制量的計算結果，按照圖6流程，完成系統(tǒng)的重力補償控制功能。

圖6 懸吊式機械臂重力補償控制流程圖

利用緩沖彈簧設備減小重力補償控制過程中可能存在的干擾力，進一步提高系統(tǒng)重力補償機構的平衡控制精度。

3 系統(tǒng)測試

以測試基于模糊PID的懸吊式機械臂重力補償控制系統(tǒng)的補償控制功能為目的，采用黑盒與白盒相結合的測試方式，設計系統(tǒng)測試實驗。

3.1 準備懸吊式機械臂實驗對象

此次實驗選擇的懸吊式機械臂研究對象的型號為HBC-200，該設備自重約500 kg，其最大負載能夠達到200 kg，最高提升高度為1.9 m，其半徑的最大值和最小值分別為2.2 m和0.5 m。采用地面螺栓固定的方式將其安裝到地面上，具體的安裝情況如圖7所示。

圖7 懸吊式機械臂實驗對象配置實景

懸吊機械臂所用的固定設備，包含懸架軌道，該懸架軌道內側可移動地連接一個安裝板，其前側設有一凹槽，在安裝板頂部與凹槽兩側安裝u型板，起到固定作用。u板頂部安裝活動插杄，且安裝板的頂部固定連接有支撐架。將準備的懸吊式機械臂與供電電源相連，通過調試保證準備的懸吊式機械臂處于正常工作狀態(tài)。除此之外，還需要準備不同重量的物體作為機械臂的作用荷載。

3.2 安裝與配置機械臂重力補償裝置

為了給系統(tǒng)的運行提供硬件支持，從卷筒、彈簧、拉絲等方面安裝重力補償裝置，具體的安裝情況如圖8所示。

圖8 重力補償裝置連接實物圖

在重力補償裝置安裝過程中，不可避免會出現(xiàn)安裝誤差，影響控制系統(tǒng)的運行效果，因此需要對相關的硬件設備進行安裝標定。以吊索滑輪設備安裝為例，在吊索自由狀態(tài)下，定義吊索與滑輪的切點是原點，吊索垂線是Z軸，Y軸是平行于滑輪軸的，與Y、Z軸垂直即為X軸，由此組成標定的右直坐標系統(tǒng)。按照上述方式完成對系統(tǒng)所有硬件設備的標定。

3.3 輸入模糊PID算法工作參數(shù)

由于優(yōu)化設計的懸吊式機械臂重力補償控制系統(tǒng)應用了模糊PID控制算法，為保證該算法的順利運行，需要設置相關的工作參數(shù)。設置比例系數(shù)、積分系數(shù)和微分系數(shù)的初始值均為0，分別將負大、負中、負小、零、正小、正中、正大設置為模糊控制算法的模糊子集，進而得出模糊規(guī)則，以此作為模糊推導環(huán)節(jié)的數(shù)據支持。

3.4 描述系統(tǒng)測試過程

在實驗開始之前，設置懸吊式機械臂的工作任務，具體的設置情況如表1所示。

表1 懸吊式機械臂實驗工作任務數(shù)據表

表1中機械臂工況的劃分依據是機械臂及其關節(jié)是否存在移動或旋轉。在安裝并調試成功的硬件環(huán)境中，導入懸吊式機械臂重力補償控制系統(tǒng)的軟件程序，為保證系統(tǒng)測試實驗的執(zhí)行安全，設定緊急停止程序，在程序發(fā)現(xiàn)張力偏差太大的情況下，兩臺電動機會同時抱死。輸入模糊PID算法工作參數(shù)以及懸吊式機械臂實時工作數(shù)據，通過軟件系統(tǒng)的運行輸出運行結果，如圖9所示。

圖9 懸吊式機械臂重力補償控制系統(tǒng)運行界面

實驗中設置傳統(tǒng)的基于軌跡規(guī)劃的懸吊式機械臂重力補償控制系統(tǒng)作為實驗的對比系統(tǒng)，按照上述流程可以得出對比系統(tǒng)重力補償控制結果。

3.5 設置系統(tǒng)測試指標

設置控制誤差與機械臂穩(wěn)定系數(shù)作為系統(tǒng)重力補償控制功能的量化測試指標。控制誤差指標的數(shù)值結果為：

ε=|utheory-ucon|

(15)

式中,utheory和ucon分別為重力補償控制的理論值和實際值，根據設置的懸吊式機械臂實驗工作任務，可以確定機械臂重力補償控制理論值的具體取值，利用機械臂上的傳感器設備可以得出ucon的取值。另外機械臂穩(wěn)定系數(shù)的測試結果可以表示為：

(16)

公式(16)中變量G為機械臂自重，Q表示起吊荷載，s和v分別表示的是幅度值和起升速度，γc為機械臂自身穩(wěn)定系數(shù)，h0和h1分別為起吊物距離地面和機械臂端的高度值，α為機械臂支撐面的傾斜角度。測試得出控制誤差越大、機械臂穩(wěn)定系數(shù)越小，說明對應系統(tǒng)的控制功能越差。

4 系統(tǒng)測試結果與分析

在系統(tǒng)中輸入相關參數(shù)，得出軟件系統(tǒng)的運行結果，統(tǒng)計數(shù)據得出兩種系統(tǒng)重力補償控制誤差結果，如表2所示。

表2 系統(tǒng)重力補償控制誤差測試結果

表2中實驗組別的劃分依據是懸吊式機械臂的工作任務，將表2中的數(shù)據代入到公式(15)中，計算得出對比系統(tǒng)的平均控制誤差為0.068 kN，而設計系統(tǒng)的控制誤差為0.012 kN。由此可知，設計系統(tǒng)的平均控制誤差更小，控制性能更佳。另外，通過公式(16)的計算可以得出兩種系統(tǒng)控制下，懸吊式機械臂穩(wěn)定系數(shù)的測試結果，如表3所示。

表3 機械臂穩(wěn)定系數(shù)測試對比結果

通過平均值計算分別得出兩種系統(tǒng)控制下，懸吊式機械臂研究對象的平均穩(wěn)定系數(shù)分別為0.81和0.95。由此可知，設計系統(tǒng)控制的穩(wěn)定系數(shù)更高，穩(wěn)定性能更佳。

綜上所述，與傳統(tǒng)重力補償控制系統(tǒng)相比，優(yōu)化設計系統(tǒng)的控制誤差更小、穩(wěn)定系數(shù)更高，即系統(tǒng)的控制功能更優(yōu)。

5 結束語

機械臂是工業(yè)生產中的重要設備之一，有效的降低了工業(yè)生產的工作難度、提高工作效率。在模糊 PID 控制算法的支持下，優(yōu)化設計懸吊式機械臂重力補償控制系統(tǒng)，系統(tǒng)測試結果證明優(yōu)化控制系統(tǒng)可以滿足機械臂的穩(wěn)定性和精確性要求。然而從實驗結果中可以看出，優(yōu)化設計的系統(tǒng)依舊存在少量控制誤差，可以通過適當增大鋼絲直徑、調整重力補償裝置與主動分離裝置之間的布局方式，進一步降低系統(tǒng)的控制誤差。此外系統(tǒng)測試未對系統(tǒng)的運行性能進行測試，因此將其應用到實際應用工作中可能會存在控制速度慢的情況，在未來的研究工作中需要對系統(tǒng)運行性能的測試數(shù)據作進一步補充。