工業機器人基于工作空間的力/位PID控制系統研究

陳浩文 ，張文輝 ，，鐘秋波 ，周書華 ，葉曉平

（1.南京工程學院自動化學院，江蘇南京211167；2.機械制造系統工程國家重點實驗室，陜西西安710000；3.浙江經濟職業技術學院汽車技術學院，浙江杭州310000；4.浙江省文創產品數字化設計與智能制造重點實驗室，浙江麗水323000）

隨著工業自動化進程的不斷發展，工業機器人將扮演越來越重要的角色，大量工業機器人被用來代替人類進行工作，以此來提高生產效率，這也使得工業機器人領域一直是研究的熱點。與傳統的工業機器人不同的是，當今工業機器人面臨打磨、拋光和裝配等任務時，單一的位置控制已經無法滿足生產的實際要求，因此能夠進行力位置混合控制的工業機器人成為了現在科研人員研究的方向。這類機器人能夠完成更多復雜的工作任務，具有更高的靈活性、安全性、智能化等特點[1]。力/位置混合控制與阻抗控制是機器人力位控制的兩種基本方法。

Mason[2]于1979年提出了同時控制力和位置的概念以及關于機械臂關節柔順的思想，并在柔順控制中引入了自然約束與人為約束的概念，將任務空間分為互補正交的力子空間和速度子空間，提出在運動約束方向上進行力控制，在力約束方向上進行位置控制。在這基礎上，1981年Raibertt與Craig[3]第一個提出了力/位置混合控制器，該控制器通過雅可比矩陣將工作空間中任意方向的力與位置分配到各個關節的控制器上，使得機器人以獨立的形式同時控制力與位置。此后楊宗泉[4]再提出了基于模糊補償的機器人力/位置控制策略。

阻抗控制是一種間接力控制。其是在阻尼控制和剛度控制的基礎上發展起來的。Hogan[5]于1987年首次系統地闡述了阻抗控制的概念，并且提出了基于機器人動力學模型的阻抗控制策略，對機器人柔順控制方向產生了深遠的影響。周曉東等[6]提出了基于粒子群算法的阻抗控制，解決了阻抗參數選取的不確定性問題，提高了系統的魯棒性。王宇馳等[7]針對機器人建模中的不確定性，提出了一種對機器人阻抗控制中不確定性進行補償的方法，能夠表現出很好的力跟蹤效果。

本文將研究工業機器人在執行復雜工作任務中的力/位置控制問題，根據關節空間中工業機器人的動力學模型推導出工作空間中動力學模型，并分別采用PID控制算法設計力/位置混合控制系統和阻抗控制系統，且證明了控制系統的穩定性。與以往研究不同，為了深入研究兩種控制方法的系統特性，本文將首次基于對等工業機器人模型，利用Matlab/Simulink進行對比數值實驗研究[8]。

1 工作空間機器人動力學建模

當N自由度機械臂末端與外界環境表面接觸時，關節空間中動力學方程為

式中：q、q、q分別為機械臂關節角度向量、角速度向量與角加速度向量；D（q）為n×n階正定慣性矩陣；C（q，q）q為 n×n 階離心力與哥氏力項；G（q）為重力矩向量；τ為關節力矩向量；J為Jacobian矩陣；F為機械臂末端與環境之間的作用力。

為了對末端位置進行控制，因此需要將關節空間中的動力學方程轉換為工作空間中的動力學方程。首先由虛功原理得

因此得到工作空間中的動力學方程

綜上所述，機械臂動力學模型是一個高度復雜、強耦合的非線性時變方程，而且其復雜程度會隨著自由度的增加而上升[9]。

2 力/位置混合控制系統設計

首先當忽略機械臂動力學模型中的重力項時，方程為

且傳統的PD控制律

下面構建李雅普諾夫函數[10]進行系統穩定性分析：

經過微分得

半負定的，所以能量是耗散的。當V˙=0 時，有θ˙=0 和θ¨=0，即e˙≡0，≡0。在此情況下，由閉環系統誤差方程可得Kpe=0。由于Kp是可逆的，因此只有e=0。由LaSalle定理可知，系統全局漸進穩定的平衡點，即從任意初始條件（θ0，θ˙0）出發，均有 θ0→θd，θ˙0→0。

力/位混合控制系統是由力控制環與位置控制環來對力與位置進行控制的，位置控制器采用PD控制（參數為Kp、Kd），力控制器采用PID控制（參數為Kfp、Kfi、Kfd）。力/位混合控制系統框圖如圖1所示。

圖1 力/位混合控制系統框圖

在工作空間中設計控制律為

3 阻抗控制系統設計

對于基于位置的阻抗控制系統來說，其主要是由位置控制器、阻抗控制模型以及被控對象的動力學模型等組成的。阻抗控制系統框圖如圖2所示。

圖2 阻抗控制系統框圖

由上面分析得到工作空間中機械臂末端與環境接觸后的動力學模型為：

根據機械手末端與環境表面接觸時產生動態接觸力作用的情況，建立如下阻抗模型：

式中xc、x為末端指令軌跡與實際軌跡，M、B和K分別為質量、阻尼與剛度系數矩陣。通過加入末端期望接觸力Fd，來計算Fd與環境表面對機械手末端的反作用力Fe之間的差值。根據阻抗模型選擇適當的阻抗系數，計算出誤差E（E為阻抗軌跡xd與輸入指令軌跡xc之間的差值），再將其與輸入的指令軌跡相加得出我們所期望的阻抗軌跡xd，即：

通過傳統的PD控制對xd與實際反饋的位置x的誤差e進行控制，設計控制律為

進而來實現機械臂的力控制。

4 控制系統實驗與分析

為了降低動力學模型的復雜程度，提高系統效率與穩定性，以二自由度工業機器人為控制對象（如圖3所示），分別采用阻抗控制與力/位混合控制兩種基本方法對機械臂力控制系統進行數值實驗研究。

圖3 二自由度工業機器人

4.1 阻抗控制

機械臂連桿長度設為l1+l2=1；在工作空間中末端的理想跟蹤軌跡取 xc=cosπt、yc=sinπt，其為一個半徑為1，圓心為（0，0）的圓；接觸環境表面的位置設在xe=0.5處，且在x軸向上的期望接觸力為Fd=10 N；阻抗系數選擇M=diag［ 1.0］、B=diag［ 10］、K=diag［ 200 ］，分別為質量、阻尼和剛度系數矩陣；位置控制器中PD參數設置：Kp=diag［ 15 ］、Kd=diag［ 10 ］。數值實驗結果如圖4～圖7所示。

圖4 末端關節的位置跟蹤

圖5 對末端軌跡的跟蹤

圖6 末端接觸力跟蹤效果

圖7 關節實際控制輸入

從圖中可以看出機械臂末端在t=0.3～1.7 s之間，未與環境表面接觸時，x與y方向上的實際軌跡均能實現對指令軌跡xc與yc的跟蹤并做逆時針圓周運動，且末端接觸力Fe恒為0N；而當t=1.8s后，在x軸方向上的位置跟蹤出現了誤差，這是因為其在x方向受到了位置約束，即設置的接觸面位置x=0.5，此時接觸力Fe上升并跟蹤為10N，之后沿著接觸面向上運動；當t=2.5s時，機械臂末端再次與接觸面分離，x方向上恢復精確的位置跟蹤，接觸力恢復為0N，此后反復循環運動。該實驗進一步驗證了各參數選擇的合理性。通過位置控制間接地實現了對末端接觸力的控制，保證了機械臂末端不會因為力過大而對接觸面或機械臂本體的結構造成破壞。

4.2 力/位置混合控制

力/位置混合控制同時也是機器人主動柔順控制的重要方法之一，主要是通過位置控制環與力控制環分別對位置和力進行控制。被控對象取同種二自由度機械臂，定義第一個連桿的質量m1和第二個連桿的質量m2都為1，連桿長度l1與l2都設為1，q為關節角度；期望末端軌跡設為工作空間的圓，即末端期望軌跡輸入為xc=cos（πt），yc=sin（πt）；接觸環境表面的位置設在 xe=0.5 處，期望力Fd=10 N，力控制器PID參數取Kp=80、Ki=2、Kd=10，位置控制器PD參數取Kp=19000、Kd=1500。根據圖1力/位混合控制結構圖，利用Simulink建立模型并仿真，數值實驗結果如圖8～圖11所示。

圖8 末端關節的位置跟蹤

圖9 對末端軌跡的跟蹤

圖10 末端接觸力跟蹤效果

圖11 關節實際控制輸入

從圖8、圖9可以看出機械臂末端在未與環境表面接觸時，機械臂能夠通過位置控制環對末端期望軌跡xc、yc進行跟蹤控制，驗證了PD控制參數選取的合理性與可靠性；當t=0.4～1.5 s之間時，機器人末端與環境表面接觸，受到了位置約束，此時機械臂末端在x方向上位置跟蹤失效，沿著接觸表面運動，通過力控制環對機械臂末端在x方向上產生的接觸力按照期望力的大小進行調節控制，使得接觸力Fe達到10 N，而在方向上依然進行位置控制；當t=2.4 s時，機械臂末端離開接觸面，此時接觸力Fe為0 N，這就實現了機械臂末端的力與位置的同時控制。

通過數值實驗對比研究，得出力/位置混合控制能夠完成對機械臂末端力與位置的跟蹤，但由于力控制器與位置控制器PID參數的合理性很難保證，使得控制器的精度達不到期望的要求，同時機械臂末端的力傳感器對末端力進行檢測并反饋時容易受外界環境的干擾而不夠精確，且該方法對任務與環境的依賴性特別大。根據外界環境的變化，機械臂需要在力控制與位置控制模式之間不停的切換，運算過程十分繁雜。

而阻抗控制的本質屬于位置控制，即間接力控制，因此避免了不確定性作用力對系統產生的干擾。從圖3、圖4可以看出，其具有較強的魯棒性，也能夠完成從無約束運動到有約束運動之間的轉換，但是阻抗模型是由阻抗參數確定的，而阻抗參數選擇的合理性很難得到，同時也無法精確獲得接觸環境的位置與剛度，這就使得系統在運行時無法避免一些誤差的產生。

5 總結

針對復雜工作任務中工業機器人力/位控制問題，推導了工作空間的工業機器人動力學模型，分別基于PID算法設計了力/位置混合控制系統和阻抗控制系統，并基于Lyapunov穩定性理論證明了控制系統的穩定性。為了深入研究兩種控制方法的系統特性，首次基于對等工業機器人模型進行了比較系統的實驗研究，研究結果表明，在期望接觸力為10 N的情況下，阻抗控制表現出更加優越的柔順性，其在力/位置控制方面精度更高，易于實現。研究結果對于相關力/位置控制技術的工程應用具有一定的借鑒意義。