2-TPR/2-TPS并聯機構的精密運動控制*

陳明方，張祥，胡康康，陳中平，張永霞

昆明理工大學機電工程學院，云南 昆明 650031

并聯機構是一個高度非線性的多輸入多輸出系統，各關節間存在著耦合慣性和干擾，且隨時變位姿參數而變化。動平臺位姿難以測量，難以搭建全閉環控制系統等因素均會影響系統的控制精度和響應特性［1-3］。為了提高并聯機構的軌跡跟蹤精度和動態性能，國內外學者針對其控制系統做了大量研究工作。魯開講等［2］提出基于模型的參考自適應控制策略，將平面3自由度并聯機構運動過程中的參數變化和未建模誤差視為干擾信號，并引入前饋和反饋環節，提高了動平臺軌跡跟蹤精度和抗干擾能力。崔學良等［4］針對冗余驅動并聯機構中冗余支鏈力阻抗控制模型隨位姿變化的問題，設計了一種模糊PID控制器，與非冗余驅動支鏈的位置控制模型組成整機的力/位混合控制模型，實現了對冗余驅動并聯機構的柔順性控制。趙勁松等［5］針對液壓并聯機構多維力加載系統非線性、時變和強耦合的系統特性，提出了一種模糊PID與小腦模型神經網絡并行的控制策略，并通過實驗證明輸出信號與指令信號幾乎沒有力/力矩幅值衰減與相位滯后，控制精度明顯提高。孔德慶等［6］針對一種3 自由度平動并聯機構因模型簡化和外部干擾對控制系統的不確定性影響，在考慮交流伺服電機動力學特性的基礎上設計了一種魯棒軌跡跟蹤策略，仿真表明該控制策略可有效抑制其不確定影響，保證系統穩定性。高國琴等［7］針對虛擬軸機床并聯機構系統模型復雜且準確建模難度大的問題，提出了一種新型自適應動態滑模控制方法，通過仿真和試驗證明其控制系統具有較好的自適應能力、較強的魯棒性和良好的動靜態性能。

PID 控制具有控制簡單、無需對被控對象建模、易于實現、可靠性高等優點，廣泛應用于機器人運動控制中。因并聯機構具有高度非線性、難以求解精確數學模型的特點，PID 控制在并聯機構控制中具有良好的適用性。隨著智能控制理論的發展，與智能算法結合的先進PID控制展現了優越的控制性能［8-10］。馮李航等［11］以Delta 系列機器人為對象，對并聯機器人控制策略進行了綜述，指出模糊控制在非線性、大干擾、無明確數學模型的并聯機構控制上具有顯著優勢。本文以2-TPR/2-TPS 并聯機構［12］為研究對象，考慮系統動態性能、各支鏈絲杠驅動力矩的時變特性以及直流無刷電機變負載條件下的運動特性等因素，搭建了基于SIMSCAPE的Electronics-Multibody速度-位移雙閉環聯合仿真控制系統，進行了傳統PID、分段PID和自適應模糊PID控制的仿真與分析。

1 機構分析

2-TPR/2-TPS 并聯機構是一種新型空間4 自由度非對稱并聯機構， 其結構如圖1 所示。R1、R2、S1、S2分別為動平臺的兩個轉動副和兩個球副，T1、T2、T3、T4分別為定平臺的4 個虎克鉸，P1、P2、P3、P4為連接動平臺和定平臺的4 條移動副支鏈，定坐標系O1x1y1z1原點O1和動坐標系O2x2y2z2原點O2分別位于線段T1T2和R1R2的中點，y1軸和y2軸分別和兩線段重合，z1軸和z2軸分別與定平臺和動平臺所在平面垂直。

圖1 2-TPR/2-TPS并聯機構簡圖Fig.1 2-TPR/2-TPS parallel mechanism diagram

實物樣機采用伺服電機驅動絲杠的方式控制4條支鏈的運動，實物模型如圖2所示。

圖2 并聯機構實物樣機Fig.2 Parallel mechanism physical prototype

1.1 路徑規劃

機器人末端路徑規劃包括3 個任務［13］：定義末端執行器在兩點之間的一條幾何曲線；定義兩個方向之間的旋轉運動；定義坐標變化在兩個給定值之間的一個時間函數。

對并聯機構進行路徑規劃，不僅要保證動平臺根據期望位姿進行運動，還要避免動平臺加速度突變帶來的關節沖擊和機構抖振，使動平臺按期望軌跡精確平滑地運動［10］。基于運動學反解的關節空間控制策略通過控制驅動桿跟蹤期望桿長位移曲線來間接跟蹤動平臺期望位姿，驅動桿位移跟蹤精度直接決定了動平臺運動精度。為了使動平臺在一定時間內精確地跟蹤期望軌跡，且各個位姿分量的速度、加速度曲線平滑，需要對動平臺運動軌跡進行合理的路徑規劃，并通過運動學反解映射到關節空間，作為并聯機構期望輸入信號。

2-TPR/2-TPS 并聯機構動平臺的6 個位姿參數具有耦合性［14］，進行路徑規劃時要根據對動平臺位姿的變化需求選取相應的獨立位姿參數。為了驗證其在笛卡爾坐標系中的平動能力，選取[x，y，z，θx]為動平臺獨立位姿參數，并在其工作空間［15］內規劃一條圓形路徑，軌跡方程為

式中x(t)，y(t)，z(t)為動坐標系原點O2相對于定坐標系原點O1的坐標分量；r0為圓軌跡半徑；θ(t)為半徑r0與定坐標系x1軸的夾角；θx(t)，θy(t)，θz(t)分別為動坐標系相對于定坐標系x1軸、y1軸、z1軸的旋轉角度。

在此路徑中，僅x(t)和y(t)是時變量，為了避免動平臺的速度、加速度突變，需對θ進行邊界條件限制，其邊界條件為

式中θt0為起始時刻θ值；θ˙t0為起始時刻速度值；θ¨t0為起始時刻加速度值；θtf為停止時刻θ值；θ˙tf為停止時刻速度值；θ¨tf為停止時刻加速度值。

五次多項式路徑可以滿足式（2）所示邊界條件，設θ(t)的表達式為

將式（2）代入式（3），可求得θ(t)和式（1）所示動平臺軌跡方程的時變函數。將式（1）代入2-TPR/2-TPS 并聯機構反解方程［14］，求得4 條驅動支鏈的位移時變函數及速度時變函數，作為控制系統期望輸入信號。

1.2 絲杠驅動力矩分析

2-TPR/2-TPS并聯機構各支鏈滑塊通過虎克鉸與定平臺連接，采用滑塊相對位置固定、絲杠旋轉的設計方案控制各支鏈的長度。機構運動過程中，各支鏈電機負荷力矩會隨著絲杠驅動力矩和絲杠角加速度的變化而變化［16-17］，整個機構是由4個子系統組成的變負載系統。

絲杠驅動力矩TD是絲杠總軸向負載Fa的函數，如式（4）所示

式中TD為絲杠驅動力矩；Fa為總軸向負載；Ph為絲杠導程；η1為絲杠和滑塊之間的傳動效率。

電機總負荷力矩TM為

式中TM為電機總負荷力矩；JM為驅動電機軸上的轉動慣量；ω˙為絲杠進給角加速度；TPL為滾珠絲杠阻力矩；Tf為滾珠絲杠副摩擦力矩；ib為電機軸與絲杠間減速轉置傳動比；η2為電機軸與絲杠間減速轉置傳動效率。

2 控制系統設計

并聯機構控制分為路徑規劃級控制和驅動執行級控制。路徑規劃級控制本質上是基于機構反解，根據末端執行器的期望路徑求解各關節理論運動指令；驅動執行級控制則是基于各關節理論運動指令，精準、快速完成實際驅動系統的伺服控制［1］。

基于動力學的控制策略對并聯機構動力學模型的準確性要求較高，對于復雜的并聯機構而言，建立精確動力學模型比較困難，由簡化模型得出的動力學反解，其關節信號與理論值存在偏差，必須通過復雜的控制算法補償，控制系統復雜且不易達到理想控制效果。基于運動學的控制策略通過機構運動學反解，可以得到關節驅動信號解析解，驗證后可直接作為驅動系統期望輸入信號，將并聯機構精度控制問題簡化為對驅動執行級的伺服系統誤差控制問題。

2-TPR/2-TPS并聯機構通過伺服電機驅動，為了滿足動平臺及絲杠運動過程中的精度和動態性能要求，采用速度驅動方式，進行速度-位移雙閉環反饋控制。控制系統總框圖如圖3所示。

圖3 控制系統總圖Fig.3 General block diagram of control system

基 于SIMSCAPE 的Electronics-Multibody 聯 合仿真控制系統如圖4所示。

圖4 Electronics-Multibody總仿真模型Fig.4 Electronics-Multibody total simulation model

2.1 分段PID控制

分段PID 控制器由多個傳統PID 控制器組成，每個子PID控制器參數需要人為整定，并根據觸發條件選擇相應的子PID控制器接入控制回路中，以實現PID控制器的自適應參數調整，滿足系統在多狀態下的穩定、快速和準確性要求，使系統總體性能達到最優，其運行機制表示為

2.2 自適應模糊PID控制

針對2-TPR/2-TPS 并聯機構運動控制過程中由位姿變化、路徑規劃函數、支鏈耦合等因素導致的系統動態特性時變的問題，需引入智能算法進行在線參數辨識，實時修改控制器參數以達到最優控制效果。自適應模糊PID控制器能根據誤差信號e和誤差變化率e˙的實時變化，基于模糊控制規則實時修改PID參數，以適應系統動態特性的變化。模糊PID控制器分為常規式和增量式，常規式直接將模糊控制器輸出作為最終PID參數；增量式在初始PID值的基礎上，將模糊控制器輸出值和初始值相加，得到最終PID 參數［18-19］，二者模糊規則不同。為了和傳統定PID控制器進行對比，選用增量式模糊PID 控制器，在相鄰采樣時間間隔內，Kp(t)、Kd(t)、Ki(t)和Δkp(t)、Δki(t)、Δkd(t)之間的關系如式（7）所示，其控制流程圖如圖5所示。

圖5 增量式模糊PID控制原理框圖Fig.5 The block diagram of incremental fuzzy PID control principle

式中Kp(t)，Ki(t)，Kd(t)為第t次采樣時系統最終PID 參數；Δkp(t)，Δki(t)，Δkd(t)為第t次采樣時模糊PID 控制器輸出參數；Kp(t- 1)，Ki(t- 1)，Kd(t- 1)為第t-1次采樣時系統最終PID參數。

2.2.1 數據模糊化 模糊控制器的輸入和輸出都是具有實際物理意義的控制信號，但模糊控制器進行運算時需要將輸入信號變換到一定尺度范圍內，輸出信號時也需要將定尺度范圍內的計算結果變換為實際物理信號，這兩個過程分別為數據的模糊化和去模糊化。輸入量e、e˙和輸出量u稱為模糊系統的語言變量，語言變量實際取值范圍稱為系統的基本論域。基本論域進行尺度變換后得到系統模糊論域，用以計算模糊向量，進行模糊運算。

為了提高調節精度，e、e˙、Δkp、Δki和Δkd的模 糊 論 域 分 別 取[-3，3]、[-3，3]、[-0.3，0.3]、[-0.06，0.06]、[-0.03，0.03]，模 糊 集 合 劃 分 為7 級，各語言變量的模糊語言值集合均表示為

2.2.2 隸屬度函數選擇 隸屬度函數的形狀對模糊控制器的性能有很大影響，需根據誤差大小、模糊概念界定等因素合理選用［20-21］。基于系統響應的快速性、穩定性和參數調節實時性的要求，在誤差較大、模糊概念完整的區間，選用S型、Z型隸屬度函數，避免參數突變，保證系統響應的平穩性；在誤差較小、具有中間模糊概念的區間，選用高斯型、三角型隸屬度函數，參數調節較靈敏，確保系統響應的快速性。e、e˙、Δkp、Δki和Δkd的隸屬度函數如圖6～10。

圖6 e隸屬度函數Fig.6 Membership function of e

圖7 e˙隸屬度函數Fig.7 Membership function of e˙

圖8 Δkp隸屬度函數Fig.8 Membership function of Δkp

圖9 Δki 隸屬度函數Fig.9 Membership function of Δki

2.2.3 模糊規則 模糊控制規則是模糊控制的核心，基于專家經驗和控制工程知識建立的模糊規則綜合了實踐經驗和PID控制理論，具有廣泛的使用場景［22］。本文選用的模糊規則如下：

圖10 Δkd隸屬度函數Fig.10 Membership function of Δkd

當|e|較大時，為使系統響應具有較好的快速跟蹤性能，避免微分飽和及較大超調，應取較大的Kp和較小的Kd，令Ki= 0。

當|e|中等大小時，為使系統具有較小的超調量，應取較小的Kp，適中的Kd和Ki，保證系統的響應速度。

當|e|較小時，為使系統具有良好的穩態性能，應取較大的Kp和Ki。同時為了避免振蕩，并考慮系統抗干擾性能，當|ec|較小時，Kd取中等值；當|ec|較大時，Kd取較小值。

根據以上PID 參數調節規則，Δkp、Δki和Δkd模糊規則表［19，23］如表1所示。

表1 Δkp、Δki、Δkd模糊規則表1）Table 1 Δkp、Δki、Δkd fuzzy rule table

2.2.4 去模糊化 模糊推理結果需進行去模糊化，轉化為模糊論域的精確量，再經尺度變化轉換為基本論域中的實際控制量，才能作用于控制系統中。常用的去模糊化方法有最大隸屬度法、加權平均法、重心法和中位數法。其中，重心法的輸出推理機制更加平滑，信號調節靈敏度更高［20-21］。為了避免調節過程中的參數突變，采用重心法進行去模糊化。重心法取隸屬度函數曲線與模糊論域軸之間的面積重心值作為該語言變量的輸出值。對于連續論域，輸出值為

式中z0為語言變量c的第i個模糊集合對應的模糊論域輸出值；μCi(z)為語言變量c的第i個模糊集合的隸屬度函數；z為隸屬度函數的自變量；a為第i個模糊集合隸屬度函數對應的論域積分下限；b為第i個模糊集合隸屬度函數對應的論域積分上限。

3 仿真分析

取r0=50 mm，z0(t)=320 mm，θx0(t)=0 rad，代入式（1），由反解方程［14］得4 條驅動支鏈的位移時變函數及速度時變函數。絲杠運動軌跡由兩部分組成，0～5 s 區間，絲杠由機構初始位置運動至目標軌跡起點位置；5～20 s區間，絲杠由目標軌跡起點運動至終點。4 條驅動支鏈絲杠位移、速度、加速度曲線如圖11～13所示。

圖11 驅動支鏈位移Fig.11 Drive chain displacement

圖12 驅動支鏈速度Fig.12 Drive chain speed

圖13 驅動支鏈加速度Fig.13 Drive chain acceleration

4 條驅動支鏈電機負載力矩曲線如圖14 所示。綜合考慮轉速跟蹤精度、機構結構特點、電機末端負載等因素，選用最大扭矩為最大負載2倍的伺服電機進行仿真。電機扭矩-速度曲線如圖15所示。

圖14 驅動支鏈電機負載Fig.14 Drive branch motor load

圖15 伺服電機扭矩-轉速包絡線Fig.15 Torque-speed envelope for the servomotor

基于臨界比例度法進行PID參數整定，并進行適當調整，分別取速度環PID 參數kp= 0.005，ki= 0.001，kd= 0.000 5，位置環kp= 1。為了減小參數整定造成的不穩定性，降低分段PID控制器的復雜度，將誤差e分為三段，分別整定對應的PID參數，其對應關系如式（9）所示。

根據傳統PID仿真的速度誤差和速度變化率誤差結果，取e和e˙的實際論域為[-7，7]、[-9，9]，在多次調節并進行效果對比的基礎上，取Δkp、Δki和Δkd的實際論域為[0.012，0.018]、[-0.001，0.001]和[0，0.005]。

傳統PID 控制、切換PID 控制和自適應模糊PID 控制下，動平臺軌跡跟蹤效果如圖16 所示。傳統PID控制下，在絲杠反向旋轉瞬間，由于轉速為零且負載方向突變，電機需要較長時間達到期望轉速，出現了明顯的軌跡偏差。分段PID 控制下，在不同的誤差區間切換相匹配的PID參數，軌跡偏差明顯減小。自適應PID 控制器調節作用下，PID 參數隨著誤差變化實時修正，在負載突變時軌跡偏差大幅減小，實際軌跡與期望軌跡重合度較高，體現了良好的動態性能。

圖16 動平臺軌跡跟蹤曲線Fig.16 Tracking curve of moving platform

為了更直觀地對比三種控制策略的調節效果，將三種控制策略下的末端軌跡半徑值和期望軌跡半徑值進行比較，得出半徑誤差曲線。如圖17 所示，傳統PID 控制下，誤差峰值為-0.96；分段PID 控制下，誤差峰值為0.51；自適應模糊PID 控制下，誤差峰值為0.47。相比傳統PID 控制器，分段PID 控制下的誤差峰值減小了47%，自適應模糊PID 控制下的誤差峰值減小了51%。雖然分段PID 控制下的誤差峰值和自適應模糊PID 控制下的相近，但是在誤差調節過程中的快速性較差，如圖17 所示，前者在兩個誤差較大的軌跡范圍內出現了較長時間的軌跡偏移，自適應模糊PID控制器作用下的軌跡半徑誤差波動幅度較小，綜合性能最優。

圖17 動平臺軌跡半徑誤差Fig.17 Trajectory radius error of moving platform

各支鏈絲杠位移誤差如圖18 所示，5～20 s 為各支鏈進行目標軌跡跟蹤的運動區間，其位移跟蹤精度直接決定了末端軌跡跟蹤精度。如圖11～14所示，5～20 s 區間內，支鏈1 和支鏈2 運動軌跡較平緩且電機負載相對較小，三種控制策略下的誤差均較小；支鏈3 和支鏈4 運動軌跡變化幅度較大且電機負載較大，三種策略的控制效果差異較大。傳統PID 控制下，支鏈3 和支鏈4 誤差峰值分別為3.3 和2.8 mm；分段PID 控制下，誤差峰值均為1.5 mm；自適應模糊PID 控制下，誤差峰值分別為0.9和1.2 mm。相對于傳統PID，分段PID 和自適應模糊PID 下的支鏈3 位移誤差分別減小了56%和72%，支鏈4 分別減小了46%和57%。自適應模糊PID下的支鏈位移誤差曲線峰值更小，波動幅度更小，體現了良好的動態跟蹤性能。

圖18 絲杠位移誤差Fig.18 Lead screw displacement error

4 結 論

通過對2-TPR/2-TPS 并聯機構的運動學仿真，可得如下結論：2-TPR/2-TPS并聯機構是一個受動平臺時變位姿、驅動絲杠角加速度、支鏈耦合等因素影響的多輸入多輸出變負載系統，負載變化和驅動絲杠反向旋轉均會降低轉速跟蹤精度，影響動平臺軌跡跟蹤精度。相比傳統定PID 控制策略，分段PID 在不同的誤差區間切換相匹配的PID控制器，具有一定的動態調節能力，自適應模糊PID 控制策略能根據電機轉速誤差e和誤差變化率e˙，實時修正PID 參數，適應負載變化，末端軌跡跟蹤精度較高，對負載時變的并聯機構具有較好的控制效果。