基于UR5機(jī)械臂的軌跡跟蹤控制算法研究

王佳　胡旭曉　吳躍成

摘? 要：為了在現(xiàn)有基礎(chǔ)上提高機(jī)械臂軌跡跟蹤控制的精度，采用自適應(yīng)模糊反演控制算法對(duì)六自由度機(jī)械臂UR5進(jìn)行軌跡跟蹤控制算法研究。用拉格朗日法對(duì)UR5機(jī)械臂進(jìn)行動(dòng)力學(xué)建模以避免系統(tǒng)的不確定因素影響。由于UR5機(jī)械臂滿足Pieper準(zhǔn)則，故對(duì)其進(jìn)行模型簡(jiǎn)化，減少運(yùn)算量，并用拉格朗日法對(duì)前兩個(gè)自由度進(jìn)行動(dòng)力學(xué)方程推導(dǎo)，該算法在角位移和角速度跟蹤方面都表現(xiàn)出了很好的性能。

關(guān)鍵詞：UR5;模型簡(jiǎn)化;拉格朗日法;軌跡跟蹤;自適應(yīng)模糊反演控制

中圖分類號(hào)：TP241.2? ? ?文獻(xiàn)標(biāo)識(shí)碼：A

Research on Trajectory Tracking Control Algorithm based on UR5 Manipulator

WANG Jia， HU Xuxiao， WU Yuecheng

Abstract： In order to improve the accuracy of manipulator trajectory tracking control on the existing basis， this paper proposes to study the trajectory tracking control algorithm of 6-DOF （degree of freedom） manipulator UR5 by using the adaptive fuzzy inversion control algorithm. The dynamic modeling of UR5 manipulator is established by Lagrange method to avoid the influence of uncertain factors of the system. Since the UR5 manipulator meets the Pieper criterion， its model is simplified to reduce the amount of computation， and the dynamic equations of the first two degrees of freedom are deduced by Lagrange method. The algorithm shows good performance in angular displacement and angular velocity tracking.

Keywords： UR5; model simplification; Lagrange method; tracking; adaptive fuzzy inversion control

1? ?引言（Introduction）

隨著工業(yè)技術(shù)的發(fā)展，機(jī)械臂逐漸成為工業(yè)領(lǐng)域自動(dòng)化水平最高的科技產(chǎn)品之一[1]。在處于確定環(huán)境下的生產(chǎn)中，它可以代替人進(jìn)行繁重的勞動(dòng)，以實(shí)現(xiàn)生產(chǎn)的機(jī)械化和自動(dòng)化并提高產(chǎn)品的質(zhì)量和生產(chǎn)效率，在有害環(huán)境下作業(yè)還能夠保障人身安全。但當(dāng)環(huán)境發(fā)生變化或變得復(fù)雜而無(wú)法精確測(cè)量機(jī)械臂的各個(gè)參數(shù)，以及難以建立精確的動(dòng)力學(xué)模型時(shí)，這些不確定因素均會(huì)限制機(jī)械臂的運(yùn)動(dòng)控制，尤其是在機(jī)械臂自主進(jìn)行接觸式操作如切割、電焊等方面將會(huì)面臨非常大的挑戰(zhàn)[2]。PID或PD控制[3]是目前工業(yè)機(jī)器人應(yīng)用最為廣泛的一種控制算法，雖易于操作但難以保證機(jī)械臂的動(dòng)態(tài)性能，當(dāng)啟動(dòng)時(shí)的輸出力矩過(guò)大時(shí)容易造成機(jī)械臂損壞。本文選用不需要精確模型的控制策略進(jìn)行跟蹤控制研究，避免了因模型建立不夠精確而降低控制精度的問題。

2? UR5機(jī)械臂動(dòng)力學(xué)模型的建立（Dynamic model establishment of UR5 manipulator）

2.1? ?UR5機(jī)械臂簡(jiǎn)介

UR5機(jī)械臂[4]是由Universal Robots公司開發(fā)的靈活、輕型的六關(guān)節(jié)工業(yè)機(jī)器人，由六個(gè)關(guān)節(jié)和擠壓鋁管組成，所有關(guān)節(jié)均為轉(zhuǎn)動(dòng)關(guān)節(jié)。關(guān)節(jié)1—6分別命名為：機(jī)座、肩部、肘部、手腕1、手腕2和手腕3。其中機(jī)座可以用來(lái)確定機(jī)器臂的安裝位置。UR5機(jī)械臂質(zhì)量較小（僅18.4 kg），可以方便地在生產(chǎn)場(chǎng)地移動(dòng)，而且不需要煩瑣的安裝與設(shè)置就可以迅速地融入生產(chǎn)線中，與員工交互合作，并可處理最大5 kg的有效負(fù)載。UR5機(jī)械臂編程可通過(guò)教學(xué)編程模式實(shí)現(xiàn)，用戶扶住機(jī)械臂，手動(dòng)引導(dǎo)機(jī)械臂按所需的路徑及移動(dòng)模式運(yùn)行一次，它就能自動(dòng)記住移動(dòng)路徑和模式。UR5機(jī)械臂可通過(guò)一套獨(dú)特的、友好的圖形用戶界面進(jìn)行操作，在觸摸屏幕上有多種功能供用戶選擇。任何重復(fù)性的生產(chǎn)過(guò)程都能夠使用它并從中受益，這使得UR5機(jī)械臂的應(yīng)用十分廣泛。

UR5機(jī)械臂前三個(gè)關(guān)節(jié)主要控制末端的位置，后三個(gè)關(guān)節(jié)主要控制末端的姿態(tài)[5]。通過(guò)各個(gè)關(guān)節(jié)之間的協(xié)調(diào)運(yùn)動(dòng)，它可以在其工作范圍內(nèi)任意移動(dòng)。UR5機(jī)械臂實(shí)物圖如圖1所示。

2.2? ?模型簡(jiǎn)化

由于通用工業(yè)機(jī)械臂滿足Pieper準(zhǔn)則[6]，即后三個(gè)關(guān)節(jié)軸相交于一點(diǎn)，因此，前三個(gè)關(guān)節(jié)主要完成機(jī)械臂末端位置運(yùn)動(dòng)，且動(dòng)力學(xué)參數(shù)值（轉(zhuǎn)動(dòng)慣量、質(zhì)量等）較大，非線性效應(yīng)比較明顯;后三個(gè)關(guān)節(jié)主要完成機(jī)械臂末端位姿調(diào)整，動(dòng)力學(xué)參數(shù)值較小。在誤差允許范圍內(nèi)，考慮將六自由度機(jī)械臂模型簡(jiǎn)化為前三個(gè)自由度機(jī)械臂模型，并將后三個(gè)關(guān)節(jié)看作動(dòng)力效應(yīng)的外部干擾，對(duì)其進(jìn)行動(dòng)力學(xué)分析。實(shí)驗(yàn)表明，當(dāng)只考慮機(jī)械臂前三個(gè)關(guān)節(jié)時(shí)其動(dòng)力學(xué)特性與考慮全部六個(gè)關(guān)節(jié)相比不失一般性，且誤差較小。這不僅減少了計(jì)算量，對(duì)于參數(shù)辨識(shí)來(lái)講，減少了待辨識(shí)的參數(shù)，也使獲得相對(duì)精確較高的動(dòng)力學(xué)模型變得更加容易。簡(jiǎn)化模型如圖2所示。

2.3? ?拉格朗日法建立動(dòng)力學(xué)方程

拉格朗日力學(xué)[7]與牛頓力學(xué)完全等效，其優(yōu)勢(shì)在于只需要寫出拉格朗日量，就可由簡(jiǎn)單求導(dǎo)得到完整的動(dòng)力學(xué)微分方程而無(wú)須進(jìn)行受力分析;方程的形式不隨廣義坐標(biāo)的選取而改變;無(wú)須解方程即可得到一些守恒量。

利用牛頓力學(xué)分析動(dòng)力學(xué)時(shí)，在分析的過(guò)程中可以知道所有連桿生成運(yùn)動(dòng)的方向以及大小，但分析太過(guò)復(fù)雜。UR5機(jī)械臂動(dòng)力學(xué)模型是一個(gè)非常復(fù)雜的非線性模型，若使用牛頓力學(xué)，出錯(cuò)的機(jī)會(huì)就會(huì)大大提高。而拉格朗日力學(xué)是基于能量項(xiàng)對(duì)系統(tǒng)變量及時(shí)間的微分方法，是以能量的角度來(lái)看待系統(tǒng)的。

歐拉-拉格朗日運(yùn)動(dòng)方程為：

（1）

其中，L是拉格朗日函數(shù)，K是系統(tǒng)動(dòng)能，P是系統(tǒng)勢(shì)能。

系統(tǒng)的拉格朗日方程為：

（2）

其中，為機(jī)械臂關(guān)節(jié)i的力矩，和為關(guān)節(jié)位置和速度。

最終寫成如下公式：

（3）

其中，M為機(jī)械臂的慣性矩陣，是與角加速度相關(guān)的項(xiàng);C為科氏力與離心力項(xiàng)矩陣，是與角速度有關(guān)的項(xiàng);G為重力項(xiàng)，不受角速度和角加速度的影響，僅與機(jī)械臂所處位置有關(guān);為外部輸入力矩，q為機(jī)械臂的角位移。

（4）

（5）

其中，A為離心力系數(shù)，B為科氏力系數(shù)。

（6）

（7）

（8）

由于機(jī)械臂的動(dòng)力學(xué)模型還受到外界干擾的影響，令外界干擾為d，動(dòng)力學(xué)模型可建立為：

（9）

假設(shè)系統(tǒng)參數(shù)是未知但有界的，且系統(tǒng)具有以下特性：

（1）慣性矩陣是正定對(duì)稱矩陣，且有界，即存在，;

（2）慣性矩陣、向心力和科氏力矩存在以下關(guān)系：

（10）

3? 自適應(yīng)模糊反演控制算法（Adaptive-fuzzy-backstepping control algorithm）

3.1? ?反演控制算法

反演控制算法是一種非線性系統(tǒng)設(shè)計(jì)的方法[8]，它是將下一級(jí)的變量當(dāng)作虛擬輸入作用在該一級(jí)系統(tǒng)上，同時(shí)根據(jù)上一級(jí)系統(tǒng)建立Lyapunov函數(shù)，實(shí)現(xiàn)系統(tǒng)穩(wěn)定，直到最后一個(gè)系統(tǒng)實(shí)現(xiàn)控制，得到輸入量。反演控制算法保證在任何情況下存在Lyapunov函數(shù)，使得該系統(tǒng)穩(wěn)定，同時(shí)利用該函數(shù)找到一個(gè)參考輸入，最終實(shí)現(xiàn)對(duì)輸入的推導(dǎo)和設(shè)計(jì)。

機(jī)械臂動(dòng)態(tài)方程可寫為：

（11）

令，則動(dòng)態(tài)方程可改寫為：

（12）

定義誤差：

（13）

（14）

式中，為虛擬控制量，通過(guò)選取，使得趨近0;為控制軌跡，為期望軌跡，為控制角速度。

?。?/p>

（15）

對(duì)于第一個(gè)子系統(tǒng)，Lyapunov函數(shù)為：

（16）

（17）

若為0，則第一個(gè)子系統(tǒng)穩(wěn)定。

（18）

控制律設(shè)計(jì)為：

（19）

對(duì)于第二個(gè)子系統(tǒng)，Lyapunov函數(shù)為：

（20）

（21）

當(dāng)時(shí)，第二個(gè)子系統(tǒng)穩(wěn)定。

反演控制[9]算法具有如下優(yōu)勢(shì)：

（1）其設(shè)計(jì)的控制器以及Lyapunov函數(shù)的過(guò)程都非常清晰明了，具有系統(tǒng)化、結(jié)構(gòu)化的特點(diǎn)。

（2）對(duì)于高階的非線性系統(tǒng)也可以取得非常好的控制效果，消除了經(jīng)典無(wú)源性設(shè)計(jì)中相對(duì)階是1的限制。

（3）不確定性要滿足匹配條件的要求，控制不確定非線性系統(tǒng)時(shí)有著明顯的優(yōu)越性。

3.2? ?自適應(yīng)模糊控制

模糊控制就是將模糊數(shù)學(xué)的基本思想和理論應(yīng)用到控制的領(lǐng)域。模糊控制[10]的一般流程包括：

（1）確定模糊控制器的結(jié)構(gòu)：確定控制器的輸入、輸出。

（2）定義輸入、輸出的模糊集。

（3）定義輸入、輸出的隸屬度函數(shù)。

（4）建立模糊控制規(guī)則。

（5）建立模糊控制集。

（6）進(jìn)行模糊推理。

傳統(tǒng)的基于模型的控制方法非常依賴于精確的動(dòng)態(tài)模型。但是對(duì)于UR5六自由度非線性的復(fù)雜系統(tǒng)來(lái)說(shuō)，是很難實(shí)現(xiàn)精確控制的。而模糊控制不依賴于被控對(duì)象精確的數(shù)學(xué)模型，適合動(dòng)態(tài)特性不易獲取的控制對(duì)象。同時(shí)，模糊控制是以人類的經(jīng)驗(yàn)積累為出發(fā)點(diǎn)的，且這種控制的魯棒性好，可以用于逼近參數(shù)不確定的各種線性和非線性系統(tǒng)的模型。

式（21）中，當(dāng)時(shí)，，此時(shí)：

（22）

將式（19）代入式（22）中得：

（23）

式（23）中，包含機(jī)器人的建模信息。由于機(jī)械臂的精確動(dòng)力學(xué)模型是很難建立的，因此我們采用自適應(yīng)模糊控制，這種控制方式不需要精確的模型信息。假設(shè)是用來(lái)逼近的模糊系統(tǒng)。這里我們采用單值模糊化、乘積推理機(jī)和重心平均反模糊化。

假設(shè)模糊系統(tǒng)由N 條模糊規(guī)則構(gòu)成，第 條模糊規(guī)則表達(dá)形式為：

： IF? is? and ... is ，then? is 。

（=1，2，…，N）

其中，為（j=1，2，…，n）的隸屬函數(shù)。

則模糊系統(tǒng)的輸出為：

（24）

其中，，，。

對(duì)于的模糊逼近，分別采用逼近（1）、（2）的形式，相應(yīng)的模糊系統(tǒng)設(shè)計(jì)為：

（25）

（26）

（27）

其中，，。

定義最優(yōu)逼近常量，對(duì)于給定的任意小的常量（>0），如下不等式成立：

設(shè)計(jì)自適應(yīng)控制律為：

（28）

4? ?實(shí)驗(yàn)結(jié)果及分析（Experiment analysis）

對(duì)UR5機(jī)械臂進(jìn)行模型簡(jiǎn)化后，并對(duì)前兩個(gè)自由度進(jìn)行軌跡跟蹤控制，采用的控制算法為自適應(yīng)模糊控制算法。

UR5機(jī)械臂動(dòng)力學(xué)參數(shù)各個(gè)連桿質(zhì)心位置及質(zhì)量如表1所示。

表1 UR5機(jī)械臂動(dòng)力學(xué)參數(shù)

Tab.1 Dynamic parameters of UR5 manipulator

連桿編號(hào) xi/m yi/m zi/m m/kg Li/m

1 0 -1.93e-3 -26.51e-3 3.7 0.892

2 212.5e-3 0 113.36e-3 8.393 0.425

本文以UR5機(jī)械臂為研究對(duì)象，對(duì)UR5機(jī)械臂進(jìn)行動(dòng)力學(xué)軌跡規(guī)劃控制。設(shè)期望軌跡分別為、

。

系統(tǒng)的初始狀態(tài)為。外界擾動(dòng)。

建立二自由度機(jī)械臂自適應(yīng)模糊反演控制系統(tǒng)Simulink仿真模型，如圖3所示，其中control為控制器子程序，D1plant為運(yùn)算實(shí)際軌跡子程序。

關(guān)節(jié)1、關(guān)節(jié)2控制軌跡與期望軌跡的跟蹤情況如圖4和圖5所示。

關(guān)節(jié)1、關(guān)節(jié)2控制角速度與期望角速度的跟蹤情況如圖6和圖7所示。

通過(guò)對(duì)機(jī)械臂的前兩個(gè)關(guān)節(jié)進(jìn)行仿真實(shí)驗(yàn)，由實(shí)驗(yàn)可知，兩個(gè)關(guān)節(jié)的角位移跟蹤精度高，說(shuō)明機(jī)械臂能按照預(yù)定軌跡進(jìn)行運(yùn)動(dòng);兩個(gè)關(guān)節(jié)的角速度跟蹤效果好，說(shuō)明機(jī)械臂能按照一定的速度進(jìn)行運(yùn)動(dòng)，不會(huì)發(fā)生突變產(chǎn)生沖擊。

5 結(jié)論（Conclusion）

本文通過(guò)對(duì)UR5機(jī)械臂進(jìn)行模型簡(jiǎn)化并分析前兩個(gè)自由度機(jī)械臂動(dòng)力學(xué)模型，設(shè)計(jì)自適應(yīng)模糊反演控制器并對(duì)其進(jìn)行誤差分析，利用MATLAB的Simulink仿真模塊實(shí)現(xiàn)機(jī)械臂各關(guān)節(jié)末端的軌跡跟蹤。仿真結(jié)果表明，位置環(huán)與速度環(huán)跟蹤的效果都很好。

參考文獻(xiàn)（References）

[1] 孫開林.六自由度工業(yè)機(jī)器人運(yùn)動(dòng)與控制技術(shù)的研究[D].無(wú)錫：江南大學(xué)，2012.

[2] 張果，趙艷花，陳文清.二自由度機(jī)械臂的魯棒軌跡跟蹤控制及仿真[J].洛陽(yáng)理工學(xué)院學(xué)報(bào)（自然科學(xué)版），2017，27（03）：62-64，83.

[3] 李巖，丁浩，孫中波，等.基于一種加速PD型迭代學(xué)習(xí)控制算法的機(jī)械臂軌跡跟蹤[J].科學(xué)技術(shù)與工程，2019，19（26）：265-269.

[4] 蔡肖肖.UR5型機(jī)器人的運(yùn)動(dòng)學(xué)分析與標(biāo)定實(shí)驗(yàn)研究[D].杭州：浙江理工大學(xué)，2017.

[5] 王海，付邦晨，薛彬，等.六自由度柔性關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)分析[J].中國(guó)機(jī)械工程，2016，27（08）：1096-1101.

[6] 晁智強(qiáng)，王飛，李華瑩，等.六自由度工業(yè)機(jī)械臂動(dòng)力學(xué)模型簡(jiǎn)化分析[J].火力與指揮控制，2018，43（02）：112-115.

[7] 宋花.六自由度機(jī)械臂軌跡跟蹤控制[D].包頭：內(nèi)蒙古科技大學(xué)，2020.

[8] 馬宇豪.六自由度機(jī)械臂避障軌跡規(guī)劃及控制算法研究[D].北京：中國(guó)科學(xué)院大學(xué)（中國(guó)科學(xué)院西安光學(xué)精密機(jī)械研究所），2019.

[9] PIKASO P， MUKHERJEE V， HINSERMU A， et al. Generalized adaptive backstepping sliding mode control for synchronizing chaotic systems with uncertainties and disturbance[J]. Mathematics and Computers in Simulation， 2021， 190（01）：793-807.

[10] CHEN Z， WANG D， DAO G H， et al. Environmental impact of the effluents discharging from full-scale wastewater treatment plants evaluated by a hybrid fuzzy approach[J]. Science of the Total Environment， 2021， 50（08）：790-798.

作者簡(jiǎn)介：

王? 佳（1998-），女，碩士生.研究領(lǐng)域：六自由度機(jī)械臂軌跡跟蹤控制.

胡旭曉（1965-），男，博士，教授.研究領(lǐng)域：圖像處理，機(jī)器人軌跡跟蹤控制.

吳躍成（1966-），男，博士，副教授.研究領(lǐng)域：人機(jī)交互.