噴砂除銹并聯機器人防空轉軌跡跟蹤控制

盧國健，高國琴，方志明

（江蘇大學電氣信息工程學院，江蘇鎮江 212013）

0 引言

鋼箱梁為大型橋梁的主要結構（見圖1），在使用前對其進行噴砂除銹等預處理可增強大橋的耐腐蝕性。目前噴砂除銹機器人主要為桁架式結構，結構復雜且龐大，難以運輸，僅適用于室內作業，與橋梁露天建設且作業地點流動性強的特點相悖。我國研制的爬壁式除銹機器人［1-2］由于結構原因承載力差、除銹效率低，不能處理復雜表面，難以勝任鋼箱梁的除銹工作。基于此，本課題組設計了一款如圖2 所示的噴砂除銹并聯機器人，其由移動平臺、升降機構、六自由度并聯操作機構和末端噴槍構成，具有工作空間大、承載性好、噴砂效率高的特點。

為確保除銹工作的穩定性和高效性，作為承重機構，移動平臺對期望路線的精確跟蹤尤為重要。文獻［3-6］對移動平臺在普通路面上的軌跡跟蹤控制進行了研究，取得了較好效果。然而，噴砂除銹造成的鋼砂堆積路面黏著系數低［7］，在移動平臺加速或經過不平整路面時，極易產生空轉現象，嚴重影響移動平臺的軌跡跟蹤精度。基于上述分析，本課題擬建立一種能夠克服鋼砂路面空轉問題的軌跡跟蹤控制算法。

1 相關研究

建立精確描述系統特性的數學模型是控制器設計的關鍵步驟，對于考慮滑移的移動平臺模型，許多學者進行了相關研究。文獻［8］將縱向滑移距離帶入運動學方程，并對滑移距離進行估計，確保了移動機器人軌跡跟蹤的精確性；文獻［9］將農田環境中的橫向與縱向距離帶入運動學方程，并對距離的位置參數設計了自適應更新率，確保了農田拖拉機的自導式行駛。上述文獻僅從運動學層面對滑移率進行了建模，無法精確描述打滑情況下移動平臺的動力學特性，難以運用于動力學控制器設計。根據滑移率特性曲線［10］，適量滑移可以給驅動輪提供足夠大的地面切向力，但過度滑移會使移動平臺切向力大幅度減小，最終導致空轉現象，嚴重影響移動平臺軌跡跟蹤精度。文獻［11］基于文獻［12］對滑移動力學特性的定量研究，將滑移現象視為驅動力的干擾項并帶入動力學方程，較為精確地描述了打滑情況下的移動平臺動力學模型，但該模型不能將滑移率設置為控制量，難以用于防空轉控制。基于此，本文根據滑移特性曲線定量計算出滑移率對驅動力的“削弱量”，建立結合滑移特性的移動平臺動力學模型。相較于上述文獻中的模型，本文模型更為精確地描述了打滑情況下的動力學特性，且可將滑移率作為控制量，為防空轉高性能控制算法研究奠定基礎。

由于移動平臺在滑移情況下模型呈高度非線性，參數多且復雜，在滑移情況下進行移動平臺軌跡跟蹤控制器設計一直是難點。文獻［13-14］利用極點搜索法尋找最佳滑移率作為期望滑移率，實現了在低附著地面上的車輛防滑控制。該方法僅采用滑移率控制解決打滑問題，未考慮軌跡跟蹤誤差，因此車體軌跡跟蹤精度不高；文獻［15］基于觀測器對滑移進行實時觀測并進行了補償；文獻［16］采用各類傳感器實現對路面參數的辨識并進行了補償。以上方法僅從運動學角度將滑移距離作為外部擾動進行補償，只能實現滑移率較小情況下的軌跡跟蹤控制，當滑移率大于最佳滑移率時，軌跡跟蹤效果不理想。基于此，為解決噴砂除銹并聯機器人移動平臺鋼砂路面軌跡跟蹤的空轉問題，本文設計了基于切換控制的防空轉高性能軌跡跟蹤控制算法。相較于上述文獻中的控制算法，本文算法既可以保證軌跡跟蹤的精確性，又能實現不同滑移程度下的防空轉控制。

基于上述分析，本文擬創新點為：①以噴砂除銹機器人移動平臺為控制對象，建立鋼砂堆積環境下考慮滑移特性的噴砂除銹并聯機器人移動平臺動力學模型，克服了以往模型精確度不高的缺點，可以精確描述該結構在空轉情況下的動力學特性；②基于上述模型設計一種防空轉高性能軌跡跟蹤控制算法，克服前人研究無法進行防滑情況下的軌跡跟蹤或僅適用于輕微滑移的缺點，可以實現在未發生空轉、發生完全空轉、發生不完全空轉三種情況下移動平臺防滑軌跡的跟蹤控制。

2 噴砂除銹并聯機器人移動平臺建模

2.1 結構簡述

噴砂除銹機器人實物圖如圖2 所示，由上至下分別為末端噴槍、六自由度并聯操作機構、升降機構、移動平臺。為避免移動平臺在鋼砂堆積路面上發生偏航角變化以及橫向打滑，移動平臺設有8 個車輪，4 個為1 組，分別控制橫向和縱向運動。橫向運動由2 個電機同步但獨立驅動，并設有電動缸控制轉向輪，保證平臺始終與鋼箱梁保持平行。

Fig.1 Physical drawing of steel box girder圖1 鋼箱梁實物圖

Fig.2 Physical picture of sandblasting and rust removal parallel robot圖2 噴砂除銹并聯機器人實物圖

2.2 移動平臺運動學模型

參考如圖3 所示的車式移動平臺X 軸方向運動示意圖，l表示前輪與后輪的軸間距，b表示車輪左右軸之間的距離。使用向量q=[x y θ φ]T表示移動平臺在世界坐標系XOY下位姿，x,y為移動平臺的位置坐標，θ為移動平臺正方向與X 軸的夾角。在不考慮移動平臺側向滑移的情況下，可以得到如下運動學方程［17］：

式中，φ為前輪轉角角度，w為前輪轉角角速度，v為移動平臺線速度，?為車輪轉速。為簡化運動學模型，定義曲率σ：

得到簡化的運動學模型為：

Fig.3 Motion diagram of the moving platform of the robot in the X-axis direction圖3 噴砂除銹并聯機器人移動平臺X 軸方向運動示意圖

假設噴砂除銹并聯機器人移動平臺只有前進方向的速度而沒有側向滑移，該移動平臺非完整約束可以表示為：

描述為矩陣的形式：

式中：

2.3 鋼砂堆積路面結合滑移特性的噴砂除銹并聯機器人移動平臺動力學模型

基于圖4 所示的驅動輪受力模型，首先建立不考慮鋼砂阻力水平、垂直受力的平衡方程和轉矩平衡方程：

式中，mw為車輪質量，v為車輪速度，Fp為車輪產生的切向力，J為車輪轉動慣量，Tm為車輪驅動力，μ為粘著系數。

Fig.4 Bekker pressure model of driving wheel圖4 驅動輪Bekker 承壓模型

如圖5 所示，當滑移率處于峰值時，移動平臺獲得牽引力最大值，當滑移率大于最佳滑移率時，驅動力會被嚴重削弱，最終導致空轉。

Fig.5 Slip characteristic curve of steel emery圖5 鋼砂滑移特性曲線

由于移動平臺縱向運動最高速度為0.14m/s，此處忽略轉動慣量，得到驅動輪電機力矩與移動平臺驅動輪牽引力的關系為：

當不發生空轉時，式（8）成立；當發生空轉時，由滑移特性曲線得到不同滑移率下車輪牽引力的實際值。滑移特性曲線可近似表示為：

式中，C1、C2、C3為鋼砂滑移特性參數。滑移率λ大于最佳滑移率λmax時，牽引力的削弱量為：

式中，Fp max為最佳滑移率對應的驅動力。

然后，根據驅動輪Bekker 承壓模型［18］計算鋼砂路面行駛阻力，地面顆粒的法向應力σ大小從θ0變化到最大值θm再到θ1=0，兩個區間的應力公式［19］為：

式中，kc為鋼砂粘聚模量，k?為鋼砂摩擦模量，n 為鋼砂變形指數。式（11）的分段函數過于復雜，為了后期控制方便，對其進行簡化與近似化處理：

近似化1：當該輪為驅動輪時，θm在θ1中點附近，經過近似化得到θm=(θ0+θ1)。

近似化2：根據鋼砂實驗圖可將上述分段函數線性化，得到：

為進一步簡化，通過近似對稱性可以將θ0到θ1的應力視為θ0到θm應力的2 倍。此時，可以得到鋼砂阻力為：

本文采用拉格朗日法建立噴砂除銹并聯機器人移動平臺動力學模型，拉格朗日函數（L）定義為系統的動能（T）與勢能（P）之差，即L=T-P。該拉格朗日方程可表示為：

式中，H(q)為非完整約束矩陣，γ為非完整約束項。

移動平臺不考慮重力勢能（U=0），故拉格朗日函數L可以描述為車體動能Lp、前輪動能Lfw、后輪動能Lrw之和，代入式（14）整理可得噴砂除銹并聯機器人移動平臺標準動力學模型為：

式中，M(q)為慣量矩陣，C(q,)為哥氏力項，N(q)為輸入轉換矩陣，τ為力矩輸出矩陣。mp和mw分別為移動平臺質量和車輪質量，Ip和Iw分別為移動平臺轉動慣量和車輪轉動慣量，τv和τσ分別為旋轉力矩和轉向力矩。

為消除非完整約束項γ，在式（15）的基礎上左乘JT，得到：

結合式（10）、式（13）和式（15），最終建立鋼砂堆積路面結合滑移特性的噴砂除銹并聯機器人移動平臺動力學模型：

式中，τx=Fx/r 為滑移導致的驅動力矩的削弱量，r為車輪半徑。

3 防空轉高性能軌跡跟蹤控制器設計

采用車載加速度計和編碼器計算滑移率誤差λe并判斷移動平臺是否發生空轉。移動平臺未發生空轉(0 <λe≤K1)時，系統進行跟蹤誤差的光滑滑模軌跡跟蹤控制（SSFSSC）；發生完全空轉(K2<λe≤1)時，系統根據鋼砂路面滑移特性確定最佳滑移率，進行滑移率誤差控制（SC）。在發生不完全空轉(K1<λe≤K2)時，系統以一定的權值系數進行SSFSSC 和SC 的復合控制。同時設計了以滑移率誤差及其導數為輸入，以復合控制權值系數為輸出的模糊規則，以降低移動平臺軌跡跟蹤過程中的滑移率誤差，提高軌跡跟蹤精度，由此實現防空轉高性能軌跡跟蹤控制（圖6 為整體控制框圖）。

Fig.6 Schematic diagram of controller圖6 控制器原理框圖

3.1 移動平臺不發生打滑時的SSFSSC

串聯/并聯機器人的動力學方程一般描述為世界坐標系位姿與關節力矩的關系。由于移動機平臺的非完整約束，只能將其動力學方程描述為移動平臺速度、曲率與驅動輪力矩的關系。因此，移動平臺的動力學控制一般采用雙閉環控制結構，即速度外環控制器以世界坐標位姿誤差為輸入，以速度和曲率為輸出；內環動力學控制器以速度外環控制器的輸出作為期望輸入，以驅動輪力矩為輸出。

由參考文獻［20］得到外環運動控制器的控制率，即動力學控制器的期望輸入Vc為：

式中，kx、ky、kθ為常數，vc≠0。

內環動力學控制器的控制目標為使移動平臺的實際速度V=[v σ]，在有限時間內跟蹤期望速度Vc=[vc σc]。

設計滑模變量S：

式中，sv(t)、sσ(t)分別為速度和曲率的滑模變量，βv、βσ為常數。當滑模變量sv(t)、sσ(t)收斂，速度跟蹤誤差ev=vc-v與曲率跟蹤誤差eσ=σc-σ也會隨之收斂。

對滑模變量函數對時間求導，得：

為求等效控制項，可令：

當移動平臺不發生打滑時，結合滑移特性的噴砂除銹并聯機器人移動平臺動力學模型中的牽引力削弱量τx=0，結合式（21）可得：

等效控制項可以保證在沒有外界隨機干擾的情況下，系統狀態在滑模面上。由于鋼砂除銹作業時存在噴槍射流反推力、鋼砂阻力等諸多不確定項，需要設計切換控制項τsw以提升系統魯棒性，取控制率τ為：

由于函數sgn(s)運用在實際控制系統中會存在嚴重的抖振問題，為減小抖振，使用連續函數s(t)代替函數sgn(s)，式（24）改寫為：

選取Lyapunov 函數：

對式（26）兩邊求導，帶入控制率（25）得到：

式中，k>0，ST?S≥0，因此V≤0。根據Lyapunov 穩定性定理，所設計的跟蹤誤差光滑滑模軌跡跟蹤控制算法穩定。

由式（13）計算出的鋼砂阻力值得到鋼砂阻力前饋補償跟蹤誤差光滑滑模軌跡跟蹤控制率為：

3.2 移動平臺發生完全打滑時的SC

定義驅動輪空轉下的滑移率為：

式中，w為驅動輪速度，由伺服電機編碼器反饋，移動平臺縱向打滑時車體速度等于驅動輪的移動速度，r為車輪半徑。對滑移率微分可得：

代入驅動輪動力學模型，可以得到滑移率λ的微分方程為：

設計滑模面s=λd-λ，為使系統在有限時間內到達切換面，選擇形式簡單、易于實現的等速趨近律：

用1 個連續函數s(t)代替函數sgn(s(t))，得到：

最終得到鋼砂路面下各個驅動輪空轉wr-v時的光滑滑模滑移率控制率：

3.3 發生不完全空轉時SSFSSC 與SC 的復合控制

當移動平臺發生不完全空轉時，驅動輪的牽引力被削弱，若此時僅進行SSFSSC，由于空轉造成的軌跡跟蹤誤差會產生較大控制力矩輸出。由滑移特性曲線可知，較大的力矩輸出會造成更嚴重的空轉。若此時僅進行SC，則不會進行軌跡跟蹤閉環控制，嚴重影響軌跡跟蹤精度。因此，本文根據文獻［21］的復合控制方法提出一種SSFSSC 與SC的復合控制，并設計以滑移率誤差及其導數為輸入，以復合控制權值系數為輸出的模糊規則。由此確定SSFSSC 和SC 的作用權重，可用式（35）來表示：

式中，Tm和Tn分別為滑移率誤差控制輸出和光滑滑模軌跡控制輸出，Tm'和Tn'為經過權值調整后的控制輸出，a(0 ≤a≤1)為復合控制的權值系數。

權值系數a的選取規則如下：

（1）當滑移率誤差小時，權值系數a取大，系統主要進行SSFSSC，并以SC 作為力矩補償。

（2）當滑移率誤差中等時，權值系數a取值中等，系統均等地進行SSFSSC 與SC。

（3）當滑移率誤差大時，權值系數a取小，系統主要進行SC，并以SSFSSC 作為力矩補償。

按照上述規則設計模糊Fuzzy 規則，具體如表1 所示。

Table 1 Weight coefficient fuzzy rule表1 權值系數模糊規則

由此得到SSFSSC 與SC 的復合控制率為：

4 仿真試驗與結果分析

為驗證本文控制算法的正確性與有效性，以式（17）所示的鋼砂堆積路面結合滑移特性的噴砂除銹并聯機器人移動平臺動力學模型為被控對象，在施加突變縱向空轉的情況下，使用SSFSSC、SSFSSC+SC 直接切換控制與結合SS?FSSC、SC、FUZZY 的防空轉高性能軌跡跟蹤控制進行MAT?LAB 仿真研究，并對比三者在突變空轉情況下的控制效果。其中，噴砂除銹并聯機器人移動平臺動力學主要參數如表2 所示。

噴砂除銹并聯機器人樣機以光柵型軌跡模擬噴砂除銹作業，需要移動平臺在X 方向進行直線運動，由此設計移動平臺的期望軌跡為：

Table 2 Moving platform parameters表2 移動平臺參數

由圖5 和式（10）可知，滑移率從最佳滑移率0.17 突變到0.5，造成的驅動力矩削弱量為99.47N。在10s 時施加上述滑移率突變，以模擬移動平臺突然發生縱向空轉的場景。此外，施加縱向外部隨機干擾d(t)=sin(3t)。由圖5所示的滑移特性曲線設置K1=0.18,K2=0.3。圖7 為移動平臺X 方向的位置跟蹤曲線，圖8 為移動平臺速度跟蹤曲線，圖9 為移動平臺滑移率跟蹤曲線，圖10 為移動平臺驅動輪驅動力矩圖。

由圖7 可知，本文提出的結合SSFSSC、SC、FUZZY 的防空轉高性能軌跡跟蹤控制以鋼砂堆積路面結合滑移特性的噴砂除銹并聯機器人移動平臺動力學模型為被控對象，在運行過程中發生嚴重空轉（滑移率從0.17 突變為0.5）的情況下，其X 方向的軌跡跟蹤效果遠優于相同條件下SS?FSSC 的控制效果，略優于SSFSSC+SC 直接切換的控制效果。

Fig.7 Position tracking curve of the X direction of the moving platform圖7 移動平臺X 方向的位置跟蹤曲線

Fig.8 Moving platform speed tracking curve圖8 移動平臺速度跟蹤曲線

Fig.9 Curve of slip rate tracking圖9 滑移率跟蹤曲線

Fig.10 Driving torque of the driving wheel of the moving platform圖10 移動平臺驅動輪的驅動力矩

由圖8、圖9 和圖10 可知，SSFSSC 控制器在發生滑移率突變時，其力矩輸出由于滑移率誤差產生驅動力而被削弱，且SSFSSC 控制器沒有滑移率控制閉環，只能增大控制力矩以達到跟蹤期望速度，但不能消除滑移率誤差。SS?FSSC+SC 直接切換控制在滑移率發生突變時切換為SC 控制，此時系統無法進行軌跡跟蹤的閉環控制。SC 控制為消除滑移率誤差，產生較低力矩輸出，雖然可以在0.28s 內消除滑移率誤差，但在此過程中造成了較大軌跡跟蹤誤差。而本文設計的結合SSFSSC、SC、FUZZY 的防空轉高性能軌跡跟蹤控制在發生大于K2的滑移率突變時，首先切換為SC控制，在約0.1s 內先將滑移率下降到0.3，然后系統進行約0.48s 的SSFSSC 與SC 的復合控制，在此過程中，系統在保證跟蹤速度的前提下消除了滑移率誤差，具有良好的軌跡跟蹤效果。

上述仿真結果表明，結合SSFSSC、SC 與FUZZY 的防空轉高性能軌跡跟蹤控制在發生嚴重空轉的情況下，不僅能快速消除滑移率誤差，而且對期望軌跡有較好的跟蹤效果，保證了移動平臺在空轉情況下的軌跡跟蹤精度。

5 結論

本文針對噴砂除銹并聯機器人移動平臺鋼砂路面軌跡跟蹤的空轉問題，提出一種結合SSFSSC、SC 與FUZZY 的防空轉高性能軌跡跟蹤控制算法，并通過仿真實驗驗證了該算法的有效性。仿真結果表明，該算法在移動平臺發生縱向突變空轉時，可在保證軌跡跟蹤效果的基礎上實現防空轉控制。本文僅研究了移動平臺縱向滑移情況下的防空轉軌跡跟蹤控制。然而在實際場景中，噴槍射流反推力以及其他外部不確定因素可能會導致橫向滑移。因此，在后續研究中可同時針對橫向和縱向滑移設計相應控制算法，以提高算法的實用性和通用性。