肖 帆 李公法 章曉峰 陶 波 蔣國璋 李 光

(1.武漢科技大學(xué)冶金裝備及其控制教育部重點實驗室，武漢 430081; 2.武漢科技大學(xué)精密制造研究院，武漢 430081;3.湖南工業(yè)大學(xué)機械工程學(xué)院，株洲 412007)

0 引言

機器人可以代替人類從事危險和重復(fù)性的工作，其已被廣泛應(yīng)用于工業(yè)、農(nóng)業(yè)、服務(wù)等行業(yè)中。機器人的任務(wù)也從搬運、碼垛等拓展到了3D打印、弧焊、噴涂等對軌跡跟蹤精度要求更高的任務(wù)中。在完成這些任務(wù)時，通常采用逆運動學(xué)[1]將機器人的笛卡爾軌跡跟蹤問題轉(zhuǎn)換為關(guān)節(jié)軌跡跟蹤控制問題。然而，由于機器人的動力學(xué)高度非線性的本質(zhì)及關(guān)節(jié)摩擦力等因素，使得機器人的軌跡跟蹤控制問題變得困難。

機器人關(guān)節(jié)軌跡跟蹤控制可分為基于模型控制和無模型控制兩種。計算力矩控制[2]是一種基于模型控制的策略，其前饋路徑能很好地抑制由于系統(tǒng)動態(tài)特性而引起的跟蹤滯后，其反饋路徑可以用來抵消不確定性引起的擾動。然而，由于結(jié)構(gòu)和非結(jié)構(gòu)的不確定性，使得精確的機器人動力學(xué)模型難以被確定，從而限制了計算力矩控制的跟蹤性能。因此，機器人動力學(xué)模型通常被分為模型部分和不確定部分。后者由其他控制方法處理，如自適應(yīng)控制[3]、魯棒控制[4-5]、滑?？刂芠6-8]、智能控制[9-11]等。

比例-微分(PD)或比例-積分-微分(PID)[12-13]是一種典型的無模型控制算法，由于其控制規(guī)律簡單，被廣泛應(yīng)用于機器的控制器中。具有重力補償?shù)腜D控制算法被用于定點跟蹤任務(wù)中時，能夠使機器人的關(guān)節(jié)位置誤差全局漸近穩(wěn)定[14-15]。然而, 重力項在實際應(yīng)用中會因載荷未知或時變而變化，不能被完全補償。因此，PD控制方案存在穩(wěn)態(tài)誤差。在PID控制中引入積分項是消除PD控制引起的靜態(tài)誤差的一種方法。在執(zhí)行定點跟蹤任務(wù)時，因不需要考慮動態(tài)因素的影響，PID可以達到很好的效果。但是在軌跡跟蹤中，PID控制方案下的跟蹤誤差會受到機器人非線性動態(tài)行為的影響。文獻[16]認為PID不適合處理高度非線性的系統(tǒng)，因為控制律的設(shè)計僅僅是基于局部參數(shù)。因此，模糊邏輯[17-19]、自適應(yīng)[20]、神經(jīng)網(wǎng)絡(luò)[21-22]等算法被用于在線調(diào)節(jié)PID的參數(shù)。雖然這些方法通過自動調(diào)整參數(shù)可以提高PID算法的跟蹤精度，但額外的計算量阻礙了它們在實時處理控制中的應(yīng)用。一些學(xué)者[23-26]基于文獻[27]，采用飽和PID控制算法使機器人系統(tǒng)穩(wěn)定。大部分機器人軌跡跟蹤控制的方法在整個控制過程中參考軌跡基本與期望軌跡保持一致。實際上，可以通過修正參考軌跡來提高機器人的軌跡跟蹤精度。

修正參考軌跡這種方法或思路，已經(jīng)被一些學(xué)者研究并成功應(yīng)用[28-34]，但是建立在傳統(tǒng)迭代學(xué)習(xí)控制的基礎(chǔ)上。該方法需要對期望軌跡重復(fù)跟蹤，然后再修正參考軌跡，耗費大量時間成本。

本文提出一種基于參考軌跡實時修正的機器人軌跡控制方法。該方法將軌跡跟蹤過程中已被跟蹤點的誤差進行累加，然后實時前饋補償至參考軌跡上即將被跟蹤的點。通過實時修正參考軌跡來實現(xiàn)機器人各個關(guān)節(jié)的實際運動符合期望軌跡。只需控制器中的算法保證速度誤差穩(wěn)定，即可保證軌跡跟蹤誤差收斂。通過仿真和實驗驗證所提方法的有效性。

1 參考軌跡實時修正方案

1.1 控制框圖

在機器人關(guān)節(jié)控制結(jié)構(gòu)中加入了前饋補償，如圖1所示。在圖中，qd表示期望的關(guān)節(jié)位置向量。它既可以表示為一個定點，也可以表示為一段連續(xù)軌跡。q表示機器人各關(guān)節(jié)當(dāng)前的位置向量。qd和q之間的誤差e稱為跟蹤誤差。Kc是一個對角矩陣，其維度與機器人自由度一致，稱為補償增益。符號∑表示累加器，它將軌跡上已被跟蹤點的Kce累加起來。累加器的輸出加到qd上，得到新的關(guān)節(jié)命令或修正后的參考軌跡qc。然后控制器根據(jù)指令誤差δq對機器人關(guān)節(jié)運動進行調(diào)整，δq是qc與q之間的誤差。當(dāng)Kc=0時，qc=qd，δq=e。

圖1 基于參考軌跡實時修正的機器人控制結(jié)構(gòu)

1.2 跟蹤誤差收斂性分析

假設(shè)每個關(guān)節(jié)的軌跡被離散為無限個點，相鄰點之間的采樣時間間隔為T。根據(jù)圖1可以得到第1個期望點與其當(dāng)前關(guān)節(jié)位置之間的誤差為

(1)

以及第1次補償后的新關(guān)節(jié)指令和指令誤差

(2)

(3)

將式(1)、(2)代入式(3)可以得到第1個點的跟蹤誤差與指令誤差間的關(guān)系

(4)

第1個點補償后，在控制器作用后，機器人各個關(guān)節(jié)到達q(2)位置。類似之前，可以得到

(5)

(6)

(7)

將式(5)、(6)代入式(7)，得

(8)

以此類推，分別得到

(9)

(10)

接下來，令式(10)減去式(9)，整理后可得

(11)

其中

En+1=δq(n+1)-δq(n)

式中I——單位矩陣

En+1——指令誤差的差分

為方便描述，利用無下標(biāo)的E表示各次補償中得到的指令誤差的差分。

將式(11)迭代展開得到

(12)

式中e1——初始狀態(tài)下關(guān)節(jié)實際位置與期望軌跡之間的誤差

從式(12)可以看出，只要Kc是正定矩陣且指令誤差的差分E等于零，跟蹤誤差e就會收斂，不受e1的影響，并且Kc中的元素值越大，收斂速度越快。

為了令式(12)具有物理意義，式(12)兩邊同時除以采樣時間T，可得

(13)

式(13)中E/T表示由指令誤差差分計算得到的指令速度誤差。由于指令速度誤差受控制器中算法的影響，因此，式(12)、(13)的一個收斂條件為控制器算法能保證指令速度誤差穩(wěn)定。

綜上所述，本文提出的方法能夠使跟蹤誤差收斂的前提是控制器中算法能夠保證速度誤差穩(wěn)定且Kc正定。其中，Kc中元素的值越大，收斂速度越快。

1.3 PD控制律證明

對于機器人，其動力學(xué)描述為

(14)

M(q)——慣性矩陣，∈Rm

g(q)——重力矩項，∈Rm

τd——外部干擾，∈Rm

u——作用于各個關(guān)節(jié)的總控制輸入，∈Rm

為了便于描述，式(14)的左邊用f替代。

圖1中控制器可采用PD控制律，其表達式為

(15)

式中KP、KD——比例和微分增益，都為正定矩陣

PD控制律穩(wěn)定性的證明可參照文獻[36-37]。文獻[36]從理論上證明了在增益KD足夠大的情況下，PD控制律可使機器人任意精度地跟蹤給定期望軌跡。然而，實際中機器人每個驅(qū)動器的轉(zhuǎn)矩有限，因此KD不可能被設(shè)置得非常大。文獻[37]證明了PD控制一致有界，這表明可以通過調(diào)整參數(shù)使軌跡跟蹤達到期望的精度。

將式(15)代入式(14)，并考慮Lyapunov函數(shù)V=δqTKPδq/2，得

(16)

控制器的采樣時間越小或者期望軌跡的速度越小，振蕩鄰域范圍會越小。這種情況下，相鄰采樣點之間的誤差變化十分接近，相當(dāng)于提高了δq調(diào)節(jié)的分辨率。因此，在控制過程中，KPδq+f的殘差也會隨之減小，系統(tǒng)也會更接近穩(wěn)定點。因為摩擦力等不是連續(xù)變化的，所以實際跟蹤中誤差可能會出現(xiàn)突變。

2 參數(shù)調(diào)節(jié)方案

在實際控制中，PD控制律通常采用離散形式，并且它的速度誤差由位置誤差差分計算得到。因此，式(15)可改寫為

(17)

由1.2節(jié)和1.3節(jié)可知，增大KP和Kc都能有效地減小最終的跟蹤誤差。從已有的PD控制律的參數(shù)調(diào)節(jié)經(jīng)驗可知，過大的KD會放大噪聲，從而引起系統(tǒng)振蕩并導(dǎo)致機器人的關(guān)節(jié)抖動。因此，所提算法主要調(diào)節(jié)KP和Kc。這兩個參數(shù)的調(diào)節(jié)方案如下：

方案1：如果系統(tǒng)受摩擦力等不連續(xù)力的影響大，則主要增加KP，可有效地降低在它們不連續(xù)區(qū)域的跟蹤誤差。

方案2：如果系統(tǒng)受摩擦力等不連續(xù)力的影響小，則主要增加Kc，可有效地縮小二者的調(diào)節(jié)范圍。

在調(diào)節(jié)KP和Kc的過程中，如果跟蹤誤差出現(xiàn)了發(fā)散性的振蕩，則適當(dāng)降低KD，再增加KP或Kc。其中KP和KD的初始值可根據(jù)已有經(jīng)驗設(shè)置，Kc的初始值可以設(shè)置為一個接近于零的值。

3 仿真和實驗驗證

3.1 期望軌跡

軌跡1用于參數(shù)調(diào)節(jié)和測試，軌跡2用于驗證所提方法對不同軌跡的適應(yīng)性以及與PID的對比，可得

qdi(t)=

軌跡2能激發(fā)機器人在慢速和快速狀態(tài)下的動力學(xué)特性[35]。

3.2 仿真

以一款只含旋轉(zhuǎn)關(guān)節(jié)的2自由度平面機器人為例。慣性矩陣表達式為

M=[mij]2×2

科氏力和向心力矩陣表達式為

C=[cij]2×2

重力矩表達式為

G=[G1G2]T

其中G1=(m1lc1+m2l1)gcosq1+m2lc2gcos(q1+q2)

G2=m2lc2gcos(q1+q2)

關(guān)節(jié)黏度/靜摩擦力矩表達式為

Fr=[Fr1Fr2]T

函數(shù)sign()表示只取輸入的正負符號。

干擾向量表達式為

τd=[τd1τd2]T

其中τd1=0.3sintτd2=0.1(1-e-t)

機器人參數(shù)分別為m1=10 kg,m2=5 kg,l1=1 m,l2=0.5 m,lc1=0.5 m,lc2=0.25 m,Ic1=0.83 kg·m2,Ic2=0.3 kg·m2,g=9.81 m/s2。所有算法采樣時間T設(shè)為0.005 s。

3.2.1軌跡1跟蹤結(jié)果

所提算法的3個參數(shù)按照第2節(jié)中的方案人工試錯調(diào)節(jié)得到，分別為KP=diag{80,10 690}，KD=diag{30,30}，Kc=diag{21,1}。第1個關(guān)節(jié)參數(shù)根據(jù)方案1調(diào)節(jié)得到，第2個關(guān)節(jié)根據(jù)方案2調(diào)節(jié)得到。

圖2為期望軌跡qd、關(guān)節(jié)實際軌跡q和實時修正后的參考軌跡qc。由圖2可以看出，修正后參考軌跡與其他兩條有很大的區(qū)別，而另外2條重合。表明本文所提出的方法在機器人軌跡跟蹤控制問題上可行。圖3為關(guān)節(jié)實際軌跡與期望軌跡之間的誤差，兩個關(guān)節(jié)誤差分別在[-0.000 3,0.003 4]rad和[-0.000 2,0.000 45]rad之間，表明所提方法有效，跟蹤精度優(yōu)。但是跟蹤誤差存在周期性的尖峰，且在初始階段尖峰值最大。周期性的尖峰是速度過零時引起摩擦力突變造成。初始階段高尖峰值則還有重力矩作用的原因。因為初始階段，所提方法作用效果是一個由弱到強的過程，因此它需要一段時間來抵消掉重力矩的影響。圖4為指令誤差差分，可以看出它與跟蹤誤差除了幅度以外，整體的變化趨勢十分接近。

圖2 關(guān)節(jié)軌跡跟蹤曲線

圖3 關(guān)節(jié)軌跡的跟蹤誤差

圖4 關(guān)節(jié)指令誤差差分

3.2.2軌跡2跟蹤結(jié)果

由于P(I)D主要通過增加比例項KP和積分項KI來提高跟蹤精度，因此，上一節(jié)設(shè)置的KD保持不變，也可更直觀地比較本文方法與P(I)D的性能。通過人工調(diào)節(jié)，P(I)D的另外兩個參數(shù)被確定為KP=diag{62 000, 41 800},KI=diag{62 000, 60 000}。

圖5為本文方法與P(I)D跟蹤軌跡2得到的跟蹤誤差。在圖5中，本文方法在前0.2 s出現(xiàn)了較大的尖峰，隨后會變周期性地出現(xiàn)較小的尖峰，但整體比較穩(wěn)定。單純的PD控制律在前1.2 s存在劇烈的振蕩，而PID的振蕩持續(xù)了約2 s。為了更好地對比性能，利用2 s后得到的跟蹤誤差絕對平均值來衡量跟蹤精度，結(jié)果見表1。由表1可知，本文方法對兩個關(guān)節(jié)的跟蹤精度比PD分別提高93.14%和98.38%；比PID分別提高79.39%和95.35%。

表1 3個算法跟蹤誤差絕對平均值

圖5 本文方法和P(I)D跟蹤軌跡2的誤差對比

圖6為3個算法計算得到的控制輸入力矩。本文方法在關(guān)節(jié)1前0.2 s內(nèi)存在一個突變，需要提供比P(I)D更大的控制輸入。隨后三者的控制輸入相差不大。P(I)D的KP較大，可以很好地抑制重力矩和摩擦力的作用，但是也會使它們在控制初始階段出現(xiàn)振蕩。

圖6 本文方法和P(I)D跟蹤軌跡2的控制輸入

綜上所述，本文方法的收斂速度、穩(wěn)定性和精度均較P(I)D更好，但是初始階段在克服重力矩影響時，需要提供更大的控制力矩。

3.2.3重力矩補償測試

根據(jù)提供的仿真模型，可將2個關(guān)節(jié)都在0 rad時計算得到的重力矩看作真實值，它們分別為110.36、12.26 N·m。實際操作中，很難準確估算出這兩個值。因此，取它們的80%作為估算值補償在式(17)右側(cè)。圖7、8分別為重力矩補償前后所提方法的跟蹤誤差和控制輸入。由圖7、8可看出，所提的這個補償方案可行且效果很好。初始階段的控制輸入顯著下降，跟蹤精度也得到了提高。

圖7 本文方法重力矩補償前后的跟蹤誤差

圖8 本文方法重力矩補償前后的控制輸入

3.3 實驗

機器人初始構(gòu)型如圖9所示，其各個關(guān)節(jié)由分辨率為0.001 5 rad/p的Dynamixel XM430-W350型舵機驅(qū)動。該款機器人總質(zhì)量僅0.7 kg，各關(guān)節(jié)受重力矩影響小。此外，其各關(guān)節(jié)受摩擦力的影響較小。因此，該機器人實驗?zāi)芨玫胤从乘岱椒ǖ挠行?。本文控制算法均在Arduino中編寫，然后被上傳至OpenCR1.0板以控制機器人的關(guān)節(jié)運動。所有算法在該控制板上的采樣時間T設(shè)為0.005 ms。

圖9 實物機器人

圖10 3個算法在實物機器人上跟蹤軌跡1的跟蹤誤差絕對值

由于各個關(guān)節(jié)的實際初始位置不處于真正的機械零位，因此對它們在0.08 s后的絕對平均值和最大絕對值進行比較，結(jié)果見表2。

表2 3個算法跟蹤軌跡1跟蹤誤差的絕對平均值和最大絕對值

圖11 3個算法在實物機器人上跟蹤軌跡2的跟蹤誤差絕對值

表3 3個算法跟蹤軌跡2得到的跟蹤誤差的絕對平均值和最大絕對值

由圖10、11可知，所提方法只需很小的參數(shù)便可達到比P(I)D更小的跟蹤誤差。軌跡2比軌跡1的速度慢，結(jié)果表明所提方法跟蹤速度越慢的期望軌跡，跟蹤誤差越小。

4 結(jié)論

(1)將軌跡跟蹤過程中已被跟蹤點的誤差累加起來作為前饋補償至即將被跟蹤的點上的方法，對機器人的軌跡跟蹤控制問題可行。

(2)在控制器中的算法使速度誤差穩(wěn)定的前提下，所提方法可以令軌跡跟蹤誤差收斂。

(3)所提方法可以起到自動調(diào)節(jié)PD控制律中參數(shù)的作用。與PID一樣有3個參數(shù)需要整定，但是收斂速度更快，跟蹤誤差更小。

(4)由于控制器采用PD算法，所提方法的跟蹤效果也會受其影響：機器人期望軌跡的速度越小，跟蹤誤差會越小；反之，跟蹤誤差也會增大。

(5)將初始位置的重力矩作為常數(shù)補償，可以有效地減小所提方法初始階段的誤差和控制輸入。