基于參考軌跡實時修正的機器人軌跡跟蹤控制方法

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

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

0 引言

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

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

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

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

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

1 參考軌跡實時修正方案

1.1 控制框圖

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

圖1 基于參考軌跡實時修正的機器人控制結構

1.2 跟蹤誤差收斂性分析

假設每個關節的軌跡被離散為無限個點，相鄰點之間的采樣時間間隔為T。根據圖1可以得到第1個期望點與其當前關節位置之間的誤差為

(1)

以及第1次補償后的新關節指令和指令誤差

(2)

(3)

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

(4)

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

(5)

(6)

(7)

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

(8)

以此類推，分別得到

(9)

(10)

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

(11)

其中

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

式中I——單位矩陣

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

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

將式(11)迭代展開得到

(12)

式中e1——初始狀態下關節實際位置與期望軌跡之間的誤差

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

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

(13)

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

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

1.3 PD控制律證明

對于機器人，其動力學描述為

(14)

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

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

τd——外部干擾，∈Rm

u——作用于各個關節的總控制輸入，∈Rm

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

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

(15)

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

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

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

(16)

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

2 參數調節方案

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

(17)

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

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

方案2：如果系統受摩擦力等不連續力的影響小，則主要增加Kc，可有效地縮小二者的調節范圍。

在調節KP和Kc的過程中，如果跟蹤誤差出現了發散性的振蕩，則適當降低KD，再增加KP或Kc。其中KP和KD的初始值可根據已有經驗設置，Kc的初始值可以設置為一個接近于零的值。

3 仿真和實驗驗證

3.1 期望軌跡

軌跡1用于參數調節和測試，軌跡2用于驗證所提方法對不同軌跡的適應性以及與PID的對比，可得

qdi(t)=

軌跡2能激發機器人在慢速和快速狀態下的動力學特性[35]。

3.2 仿真

以一款只含旋轉關節的2自由度平面機器人為例。慣性矩陣表達式為

M=[mij]2×2

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

C=[cij]2×2

重力矩表達式為

G=[G1G2]T

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

G2=m2lc2gcos(q1+q2)

關節黏度/靜摩擦力矩表達式為

Fr=[Fr1Fr2]T

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

干擾向量表達式為

τd=[τd1τd2]T

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

機器人參數分別為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設為0.005 s。

3.2.1軌跡1跟蹤結果

所提算法的3個參數按照第2節中的方案人工試錯調節得到，分別為KP=diag{80,10 690}，KD=diag{30,30}，Kc=diag{21,1}。第1個關節參數根據方案1調節得到，第2個關節根據方案2調節得到。

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

圖2 關節軌跡跟蹤曲線

圖3 關節軌跡的跟蹤誤差

圖4 關節指令誤差差分

3.2.2軌跡2跟蹤結果

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

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

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

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

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

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

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

3.2.3重力矩補償測試

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

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

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

3.3 實驗

機器人初始構型如圖9所示，其各個關節由分辨率為0.001 5 rad/p的Dynamixel XM430-W350型舵機驅動。該款機器人總質量僅0.7 kg，各關節受重力矩影響小。此外，其各關節受摩擦力的影響較小。因此，該機器人實驗能更好地反映所提方法的有效性。本文控制算法均在Arduino中編寫，然后被上傳至OpenCR1.0板以控制機器人的關節運動。所有算法在該控制板上的采樣時間T設為0.005 ms。

圖9 實物機器人

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

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

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

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

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

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

4 結論

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

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

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

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

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