擦玻璃機器人的自適應阻抗控制研究

尹恒健，倪受東

（南京工業大學機械與動力工程學院，江蘇 南京 211800）

1 引言

隨著社會的不斷發展，高樓建筑越來越多，玻璃幕墻因采光好、美觀的特點被廣泛的使用，同時清洗高空玻璃的工作變得復雜。一般情況下，清洗高空玻璃的工作有兩種方式：人工清洗和升降機清洗。人工清洗會帶來一定的危險性[1]，升降機清洗成本較高以及容易出現清洗的死角。為了解決這些顧慮，有些學者也在研究。例如最近幾年，文獻[2]提出的“基于涵道推力式擦玻璃機器人”的研究，雖然在自由行走時可以克服摩擦，保證機器人不會滑落，但此類擦玻璃機器人結構復雜且成本較高；文獻[3]提出的“一種輪足組合式擦玻璃機器人”，大大提高清洗的穩定性，缺點就是自帶的水箱重量大，無法實現長時間的清洗。綜上，難以解決的兩大問題就是清洗時機器人末端的作業位置以及輸出的末端接觸力是否被外力所干擾。

早在1985年，文獻[4]提出的“阻抗控制”這一概念，將位置和力納入一個動態框架，從補償性的角度解決了機器人在運動時易受外界干擾的問題，但由于對機器人模型的精確度要求極高，在作業中很難達到預期的效果。后來Seul提出的“自適應阻抗控制”，是在Hogan研究的基礎上，增加了延時模塊，擁有了較好的魯棒性[5]。最近幾年，自適應阻抗算法多運用于醫療領域，例如基于下肢康復機器人、上肢輔助外骨骼機器人、可穿戴機器人等等，這些機器人的產生，一定程度上減輕了老人以及在事故中受傷的殘疾人士的痛苦[6]。與此同時，隨著工業領域的需求不斷增多，自適應阻抗控制算法的優越性逐漸顯示出來。

本研究提出的擦玻璃機器人是在傳統工業機器人的基礎上，引用了自適應阻抗算法，在機器人處于未知環境時，對機器人末端的位置和力進行一定的規劃，減小外界的干擾，確保玻璃達到預期的清潔效果。

2 機器人平臺搭建

這里研究的擦玻璃機器人模型平臺包括PUMA560機器人、末端清潔器、支撐板，如圖1所示。末端清潔器主要由小型驅動器、力傳感器以及清潔海綿構成；支撐板水平放置，主要是為了減少機器人在工作工程中左右晃動的現象。當機器人末端的位置和接觸力達到預期的數值，玻璃的清洗工作就可以正常進行。

圖1 擦玻璃機器人的主要結構Fig.1 Main Structure of the Glass Cleaning Robot

2.1 PUMA560機器人模型的建立

為了完成擦玻璃的任務，選擇的理想機器人應具有簡便和易于操作的特點，而PUMA560機器人是工業生產中較為常見的機器人，符合這項任務的需求。它有六個轉動關節，前面三關節控制了大體的運動方向，后面三關節確定末端的具體位置，這樣可以展開六自由度的作業。如表1所示，在D-H 表中，?i表示扭轉角，ai表示橫距，di表示連桿長度，θi表示關節角，利用MATLAB中的robotics 工具箱畫出PUMA560 機器人的三維模型，如圖2所示。

表1 PUMA560機器人的D-H參數Tab.1 D-H Parameters of PUMA560 Robot

圖2 三維空間中的PUMA560機器人Fig.2 PUMA560 Robot in the Three-Dimensional Space

為了便于下面的研究，從輸出力矩的角度考慮，取連桿2和連桿3構成兩連桿機械臂進行深入的研究，如圖3所示。兩連桿機械臂的坐標表示，得出末端機械臂的坐標表示為：

圖3 兩連桿機械臂坐標系Fig.3 Two Link Manipulator Coordinate System

式中：θ1—關節角1；θ2—關節角2；L1和L2—機械臂的長度。

根據雅可比矩陣的定義，末端的坐標值對θ1，θ2分別求偏導，由此推出雅可比矩陣為：

2.2 拉格朗日動力學分析

在工業機器人研究領域，拉格朗日方程為：

式中：L—拉格朗日函數；τi—廣義力；θi—變量；—變量的一階導數。根據前面建立的兩連桿機械臂的坐標表示，分別建立整個系統動能和勢能的表達式[7]，得出拉格朗日函數：

n自由度的機器人動力學方程為：

式中：D(θ)—慣性矩陣；C(θ,)—離心力 哥氏力矢量矩陣；G(θ,θ)—重力矢量矩陣。由上面兩式結合，得出：

3 自適應阻抗控制器的設計

與傳統的阻抗控制算法相比，自適應阻抗控制算法在位置環境下表現出一定的適應性，及時地對力反饋信息進行處理。

更重要的是，自適應阻抗算法不需要對機器人建立精確的模型，在實際的工業生產中表現出良好的魯棒性，能夠把機器人與環境之間的接觸力和實際的位置穩定在期望值范圍內。

3.1 自適應阻抗算法控制器的原理

在外部環境作用下的n自由度機器人的動力學表達式如下：

在傳統阻抗控制的基礎上，建立新的阻抗的關系式如下：

式中：Fe—機器人與環境的接觸力；Fd—期望力；M，B，K—慣性矩陣、阻尼矩陣、剛度矩陣；E—期望位置Xd和實際位置X之差。

式中：為了后續研究的簡便，將參數Fd，Fe，M，B，K對應為fd，fe，m，b，k。

由于在實際的過程當中，自由空間下的狀態是不存在的，所以只考慮接觸空間這類情況。

仿真中沒有傳感器，所以將剛度系數設置為零，在力控方向上假設：

式中：ke—環境剛度。

其中，考慮到實際的工作環境，要對環境位置有個大概的估計值Xe，設e′=e+δXe。

為了保證力誤差的穩定性，自適應阻抗方程為：

由式得出，自適應阻抗算法的框架，如圖4所示。

圖4 自適應阻抗原理框圖Fig.4 Adaptive Impedance Schematic Diagram

3.2 自適應阻抗算法的仿真研究

假設兩連桿機械臂的兩關節長度都為1m，質量都為1kg得出：

故特征值下界ρ為0.172，由于估計矩陣且0 ＜α＜2ρ，所以得出：0 ＜α＜0.344。為了方便后續與粒子群算法優化后的仿真結果作對比，下面仿真只針對X方向上的接觸力（允許有0.01的誤差），自適應阻抗控制仿真模型，如圖5所示。

圖5 自適應阻抗算法的simulink仿真模型Fig.5 Simulink Simulation Model of Adaptive Impedance Algorithm

仿真模型中，6個S函數分別表示笛卡爾空間內X，Y的坐標、、ROBOT模塊（輸入總力矩，求出逆解得出角度和角速度）、延時模塊，根據圖中的原理框圖依次搭建好simulink模塊。

其中延時模塊的內部結構，如圖6所示。

圖6 延時模塊Fig.6 Delay Module

根據圖7 所示，在X，Y方向上加入階躍信號分別為Xd=0.51,Yd=1.866，將期望力控制在15N，環境信號（規劃的位置）X=0.5，Y=1.866，仿真模型中三個阻抗參數分別設置為：

圖7 機械手的初始位置Fig.7 Initial Position of Manipulator

圖8 X方向上的位置Fig.8 Position in the X Direction

圖9 X方向上的力Fig.9 Force in the X Direction

由上圖可以看出，X方向的位置始終在誤差范圍之類，最大的實際的接觸力達到25.5364N，經過1.618s，接觸力穩定在15N。綜上分析，采用自適應阻抗算法設計控制器，不僅能有效地實現位置控制，對機械臂末端的接觸力有明顯的控制效果。

4 粒子群算法優化參數

在這里，采用粒子群算法主要是優化自適應阻抗算法中的M，B，K三個參數，不斷的更新粒子的速度和位置，從而使整個自適應阻抗算法達到理想的效果[10]。適應度函數決定粒子的優劣采用的函數如下：

式中：μ1—力誤差，表示位置誤差；ρ—常數，范圍是（0～1）。

4.1 搭建粒子群算法（PSO）自適應阻抗控制平臺

Fcn模塊力包含適應度函數，u1表示期望位置和實際位置的差，u2表示期望力和實際力的差，p為一個0到1的常數，Out表示適應度值的輸出，如圖10所示。

圖10 粒子群算法優化的自適應阻抗控制器Fig.10 Particle Swarm Optimization Adaptive Impedance Controller

為了讓粒子群算法中的主函數和適應度函數連接起來，需要用到以下腳本語句：

將粒子群算法生成的粒子通過Assignin函數賦值給仿真模型中的M，B，K三個參數，進行離線運算。在算法的主程序中，粒子的維數設置為3，粒子數目為20，迭代次數為100，加速參數都為2，慣性參數為0.6，粒子的速度為[-1，1]，粒子上下限分別為[0，40，500]，[2，100，70]。結果，如圖11所示。

圖11 適應度值迭代曲線Fig.11 Fitness Value Iteration Curve

由上圖可以看出，迭代次數達到32時，適應度值完全收斂，此時選取的M，B，K的值為最優值，分別為1.0292，87.7710，595.9844。X方向上的力，如圖12所示。

圖12 粒子群算法優化后的X方向上的力Fig.12 Particle Swarm Optimization in the X Direction of the Force

和前面的自適應阻抗仿真模型相比（因為兩者都能使X方向上的位置穩定在誤差之內，故不作討論），PSO優化后的結果是經過1.163s，接觸力穩定在15N，提前了0.455s。這表明經過粒子群優化后的自適應阻抗控制器穩定時間縮短，起到了明顯的優化效果。

4.2 測量PSO-自適應阻抗控制器承受的范圍

上文中的接觸力15N顯然不是PSO-自適應阻抗控制器承受的最大極限值，為了進一步研究此控制器，將15N的力慢慢疊加，過程中出現了兩個關鍵的節點數據，分別是23N和500N時候機器人末端的位置和力的控制情況，如圖13、圖14所示。仿真結果表明，在一步步增大期望力的時候，期望力在（0～23）N時，機器人末端位置和接觸力都能被有效的控制；期望力超過500N時，仿真過程中開始報錯，即控制器最大承受力為500N，但位置無法被限制誤差范圍之內，超過理想位置約0.2m。

圖13 兩種情況下X方向上的力Fig.13 The Force in the X Direction in the Both Cases

圖14 兩種情況下X方向上的位置Fig.14 The Position in the X Direction in Both Cases

因此，在現實的玻璃清洗過程中，對一些剛度較小且清潔度要求高的玻璃（例如住宅區的裝飾玻璃），通過將此控制器的期望力設置在（0～23）N之內，可以起到好的效果；對一些剛度高且工作量大的玻璃（例如玻璃幕墻），可以將此控制器的期望力設置在（23～500）N之內，此控制器都可以滿足相應的需求。

5 結論

這里采用自適應阻抗算法研究擦玻璃機器人，在未知的環境下，有效地對機械臂末端的位置和接觸力進行控制，在實際的擦玻璃工作中可以有效減少了外界的干擾。

通過引入了粒子群算法，對自適應阻抗參數進行優化。尤其是測量控制器模型承受的最大期望力，在實際生活在中有較大的應用價值。

但研究的過程中仍存在許多不足：針對擦玻璃機器人輸出力矩較大的連桿2、3進行研究，對比于6自由度機器人而言，達不到最理想的結果；未立足于實際條件來考慮到關節之間的摩擦因數；沒有對粒子群算法進行改進，粒子群易容易陷入局部最優。在今后研究過程中，筆者將重點研究多自由度機器人的研究，立足于實驗，對干擾擦玻璃機器人作業的實際因數進行更多的探討。