基于參考軌跡實(shí)時(shí)修正的機(jī)器人軌跡跟蹤控制方法

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

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

0 引言

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

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

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

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

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

1 參考軌跡實(shí)時(shí)修正方案

1.1 控制框圖

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

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

1.2 跟蹤誤差收斂性分析

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

(1)

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

(2)

(3)

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

(4)

第1個(gè)點(diǎn)補(bǔ)償后，在控制器作用后，機(jī)器人各個(gè)關(guān)節(jié)到達(dá)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表示各次補(bǔ)償中得到的指令誤差的差分。

將式(11)迭代展開得到

(12)

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

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

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

(13)

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

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

1.3 PD控制律證明

對(duì)于機(jī)器人，其動(dòng)力學(xué)描述為

(14)

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

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

τd——外部干擾，∈Rm

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

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

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

(15)

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

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

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

(16)

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

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

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

(17)

由1.2節(jié)和1.3節(jié)可知，增大KP和Kc都能有效地減小最終的跟蹤誤差。從已有的PD控制律的參數(shù)調(diào)節(jié)經(jīng)驗(yàn)可知，過大的KD會(huì)放大噪聲，從而引起系統(tǒng)振蕩并導(dǎo)致機(jī)器人的關(guān)節(jié)抖動(dòng)。因此，所提算法主要調(diào)節(jié)KP和Kc。這兩個(gè)參數(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)驗(yàn)設(shè)置，Kc的初始值可以設(shè)置為一個(gè)接近于零的值。

3 仿真和實(shí)驗(yàn)驗(yàn)證

3.1 期望軌跡

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

qdi(t)=

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

3.2 仿真

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

M=[mij]2×2

科氏力和向心力矩陣表達(dá)式為

C=[cij]2×2

重力矩表達(dá)式為

G=[G1G2]T

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

G2=m2lc2gcos(q1+q2)

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

Fr=[Fr1Fr2]T

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

干擾向量表達(dá)式為

τd=[τd1τd2]T

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

機(jī)器人參數(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。所有算法采樣時(shí)間T設(shè)為0.005 s。

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

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

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

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

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

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

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

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

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

表1 3個(gè)算法跟蹤誤差絕對(duì)平均值

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

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

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

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

3.2.3重力矩補(bǔ)償測試

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

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

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

3.3 實(shí)驗(yàn)

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

圖9 實(shí)物機(jī)器人

圖10 3個(gè)算法在實(shí)物機(jī)器人上跟蹤軌跡1的跟蹤誤差絕對(duì)值

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

表2 3個(gè)算法跟蹤軌跡1跟蹤誤差的絕對(duì)平均值和最大絕對(duì)值

圖11 3個(gè)算法在實(shí)物機(jī)器人上跟蹤軌跡2的跟蹤誤差絕對(duì)值

表3 3個(gè)算法跟蹤軌跡2得到的跟蹤誤差的絕對(duì)平均值和最大絕對(duì)值

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

4 結(jié)論

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

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

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

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

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